Remove unused files (nn, nv, sample)

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

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 41197b301a6732c0632f8ac67fccb8c47fe8dec5..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
@@ -330,7 +335,7 @@ bool RRTBase::link_obstacles(
 
 bool RRTBase::add_iy(RRTNode *n)
 {
-        int i = IYI(n->y());
+        int i = this->YI(n);
         if (i < 0)
                 i = 0;
         if (i >= IYSIZE)
@@ -341,12 +346,12 @@ bool RRTBase::add_iy(RRTNode *n)
 
 bool RRTBase::add_ixy(RRTNode *n)
 {
-        int ix = IXI(n->x());
+        int ix = this->XI(n);
         if (ix < 0)
                 ix = 0;
         if (ix >= IXSIZE)
                 ix = IXSIZE - 1;
-        int iy = IYI(n->y());
+        int iy = this->YI(n);
         if (iy < 0)
                 iy = 0;
         if (iy >= IYSIZE)
@@ -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
                                 );
                         }
                 }
@@ -1001,7 +1008,7 @@ bool RRTBase::rebase(RRTNode *nr)
                 }
                 if (to_del < this->nodes_.size())
                         this->nodes_.erase(this->nodes_.begin() + to_del);
-                iy = IYI(tmp->y());
+                iy = this->YI(tmp);
                 to_del = this->iy_[iy].size();
                 #pragma omp parallel  for reduction(min: to_del)
                 for (i = 0; i < this->iy_[iy].size(); i++) {
@@ -1034,6 +1041,24 @@ std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
         return nodes;
 }
 
+int RRTBase::XI(RRTNode *n)
+{
+        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;
+}
+
+int RRTBase::YI(RRTNode *n)
+{
+        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
 void RRTBase::setSamplingInfo(SamplingInfo si)
 {
@@ -1057,7 +1082,7 @@ float RRTBase::cost(RRTNode *init, RRTNode *goal)
 
 RRTNode *RRTBase::nn(RRTNode *rs)
 {
-        int iy = IYI(rs->y());
+        int iy = this->YI(rs);
         struct mcnn nn;
         nn.nn = nullptr;
         nn.mc = 9999;
@@ -1095,7 +1120,7 @@ RRTNode *RRTBase::nn(RRTNode *rs)
 std::vector<RRTNode *> RRTBase::nv(RRTNode *node, float dist)
 {
         std::vector<RRTNode *> nvs;
-        unsigned int iy = IYI(node->y());
+        unsigned int iy = this->YI(node);
         unsigned int iy_dist = floor(dist / IYSTEP) + 1;
         unsigned int i = 0; // vector index
         unsigned int j = 0; // array index