Remove unused files (nn, nv, sample)

[hubacji1/iamcar.git] / base / rrtbase.cc

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 09cc0f6eed02153e9d792b158a781efca56bb70e..8785ad7230849e6b3f736d5d6df3610f092892d1 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -30,11 +30,8 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #endif
 
 // RRT
-#include "sample.h"
 #include "cost.h"
 #include "steer.h"
-#include "nn.h"
-#include "nv.h"
 
 #if USE_GL > 0
 extern SDL_Window* gw;
@@ -95,28 +92,36 @@ RRTBase::RRTBase()
         : root_(new RRTNode())
         , goal_(new RRTNode())
         , gen_(std::random_device{}())
-        , ndx_(HMAX - HMIN, (HMAX - HMIN) / 4)
-        , ndy_(VMAX - VMIN, (VMAX - VMIN) / 4)
-        , ndh_(0, M_PI * 2 / 4)
 {
         this->nodes_.reserve(NOFNODES);
         this->nodes_.push_back(this->root_);
         this->add_iy(this->root_);
         this->add_ixy(this->root_);
+        float hcenter = (this->HMAX - this->HMIN) / 2 + this->HMIN;
+        float hrange = (this->HMAX - this->HMIN) / 2;
+        float vcenter = (this->VMAX - this->VMIN) / 2 + this->VMIN;
+        float vrange = (this->VMAX - this->VMIN) / 2;
+        this->ndx_ = std::normal_distribution<float>(hcenter, hrange);
+        this->ndy_ = std::normal_distribution<float>(vcenter, vrange);
+        this->ndh_ = std::normal_distribution<float>(0, 2 * M_PI);
 }
 
 RRTBase::RRTBase(RRTNode *init, RRTNode *goal)
         : root_(init)
         , goal_(goal)
         , gen_(std::random_device{}())
-        , ndx_(HMIN + (HMAX - HMIN) / 2, (HMAX - HMIN) / 4)
-        , ndy_(VMIN + (VMAX - VMIN) / 2, (VMAX - VMIN) / 4)
-        , ndh_(0, M_PI * 2 / 4)
 {
         this->nodes_.reserve(NOFNODES);
         this->nodes_.push_back(init);
         this->add_iy(init);
         this->add_ixy(init);
+        float hcenter = (this->HMAX - this->HMIN) / 2 + this->HMIN;
+        float hrange = (this->HMAX - this->HMIN) / 2;
+        float vcenter = (this->VMAX - this->VMIN) / 2 + this->VMIN;
+        float vrange = (this->VMAX - this->VMIN) / 2;
+        this->ndx_ = std::normal_distribution<float>(hcenter, hrange);
+        this->ndy_ = std::normal_distribution<float>(vcenter, vrange);
+        this->ndh_ = std::normal_distribution<float>(0, 2 * M_PI);
 }
 
 // getter
@@ -373,6 +378,8 @@ void RRTBase::slot_cusp(std::vector<RRTNode *> sc)
 bool RRTBase::glplot()
 {
 #if USE_GL > 0
+        float glplwscale = 1.0 / ((this->VMAX) - (this->VMIN));
+        float glplhscale = 1.0 / ((this->HMAX) - (this->HMIN));
         glClear(GL_COLOR_BUFFER_BIT);
         glLineWidth(1);
         glPointSize(1);
@@ -415,28 +422,28 @@ bool RRTBase::glplot()
                                 glColor3f(0.5, 0.5, 0.5);
                                 BicycleCar bc(tmp->x(), tmp->y(), tmp->h());
                                 glVertex2f(
-                                        bc.lfx() * GLPLWSCALE,
-                                        bc.lfy() * GLPLHSCALE
+                                        bc.lfx() * glplwscale,
+                                        bc.lfy() * glplhscale
                                 );
                                 glVertex2f(
-                                        bc.lrx() * GLPLWSCALE,
-                                        bc.lry() * GLPLHSCALE
+                                        bc.lrx() * glplwscale,
+                                        bc.lry() * glplhscale
                                 );
                                 glVertex2f(
-                                        bc.lrx() * GLPLWSCALE,
-                                        bc.lry() * GLPLHSCALE
+                                        bc.lrx() * glplwscale,
+                                        bc.lry() * glplhscale
                                 );
                                 glVertex2f(
-                                        bc.rrx() * GLPLWSCALE,
-                                        bc.rry() * GLPLHSCALE
+                                        bc.rrx() * glplwscale,
+                                        bc.rry() * glplhscale
                                 );
                                 glVertex2f(
-                                        bc.rrx() * GLPLWSCALE,
-                                        bc.rry() * GLPLHSCALE
+                                        bc.rrx() * glplwscale,
+                                        bc.rry() * glplhscale
                                 );
                                 glVertex2f(
-                                        bc.rfx() * GLPLWSCALE,
-                                        bc.rfy() * GLPLHSCALE
+                                        bc.rfx() * glplwscale,
+                                        bc.rfy() * glplhscale
                                 );
                         }
                 }
@@ -1034,14 +1041,22 @@ std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
         return nodes;
 }
 
-float RRTBase::XI(RRTNode *n)
+int RRTBase::XI(RRTNode *n)
 {
-       return n->x();
+        float step = (this->HMAX - this->HMIN) / IXSIZE;
+        float index = (int) (floor(n->x() - this->HMIN) / step);
+        if (index < 0) index = 0;
+        if (index >= IXSIZE) index = IXSIZE - 1;
+        return index;
 }
 
-float RRTBase::YI(RRTNode *n)
+int RRTBase::YI(RRTNode *n)
 {
-       return n->y();
+        float step = (this->VMAX - this->VMIN) / IYSIZE;
+        float index = (int) (floor(n->y() - this->VMIN) / step);
+        if (index < 0) index = 0;
+        if (index >= IYSIZE) index = IYSIZE - 1;
+        return index;
 }
 
 // RRT Framework