Remove unused files (nn, nv, sample)

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

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 7a7c194d3e7df5519b2cd4cb223bc037cce30745..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,14 +1082,45 @@ float RRTBase::cost(RRTNode *init, RRTNode *goal)
 
 RRTNode *RRTBase::nn(RRTNode *rs)
 {
-        return nn4(this->iy_, rs, nullptr);
-        //return nn3(this->iy_, rs, nullptr);
+        int iy = this->YI(rs);
+        struct mcnn nn;
+        nn.nn = nullptr;
+        nn.mc = 9999;
+        unsigned int i = 0; // vector step
+        unsigned int j = 0; // array step
+        int iyj = 0;
+        while (nn.mc > j * IYSTEP) {
+                iyj = (int) (iy + j);
+                if (iyj >= IYSIZE)
+                        iyj = IYSIZE - 1;
+                #pragma omp parallel for reduction(minn: nn)
+                for (i = 0; i < this->iy_[iyj].size(); i++) {
+                        if (EDIST(this->iy_[iyj][i], rs) < nn.mc) {
+                                nn.mc = EDIST(this->iy_[iyj][i], rs);
+                                nn.nn = this->iy_[iyj][i];
+                        }
+                }
+                if (j > 0) {
+                        iyj = (int) (iy - j);
+                        if (iyj < 0)
+                                iyj = 0;
+                        #pragma omp parallel for reduction(minn: nn)
+                        for (i = 0; i < this->iy_[iyj].size(); i++) {
+                                if (EDIST(this->iy_[iyj][i], rs) < nn.mc) {
+                                        nn.mc = EDIST(this->iy_[iyj][i], rs);
+                                        nn.nn = this->iy_[iyj][i];
+                                }
+                        }
+                }
+                j++;
+        }
+        return nn.nn;
 }
 
 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