Fix findt() for multiple goals

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

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 516504ef3a6fff99e4a81825acdbe469279ef75c..dbcf838f74f7887133a1ab34b03cc3f476241e38 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
                                 );
                         }
                 }
@@ -1019,7 +1026,14 @@ bool RRTBase::rebase(RRTNode *nr)
 
 std::vector<RRTNode *> RRTBase::findt()
 {
-        return this->findt(this->goal_);
+        RRTNode *goal = this->goal_;
+        for (auto g: this->goals()) {
+                if (goal->parent() == nullptr || g->ccost() < g->ccost())
+                        goal = g;
+        }
+        if (goal->parent() == nullptr)
+                this->goal_found(false);
+        return this->findt(goal);
 }
 
 std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
@@ -1038,6 +1052,8 @@ 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;
 }
 
@@ -1045,10 +1061,23 @@ 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::defaultSamplingInfo()
+{
+        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);
+}
+
 void RRTBase::setSamplingInfo(SamplingInfo si)
 {
         this->ndx_ = std::normal_distribution<float>(si.x0, si.x);
@@ -1072,13 +1101,14 @@ float RRTBase::cost(RRTNode *init, RRTNode *goal)
 RRTNode *RRTBase::nn(RRTNode *rs)
 {
         int iy = this->YI(rs);
+        float iy_step = (this->VMAX - this->VMIN) / IYSIZE;
         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) {
+        while (nn.mc > j * iy_step) {
                 iyj = (int) (iy + j);
                 if (iyj >= IYSIZE)
                         iyj = IYSIZE - 1;
@@ -1110,7 +1140,8 @@ std::vector<RRTNode *> RRTBase::nv(RRTNode *node, float dist)
 {
         std::vector<RRTNode *> nvs;
         unsigned int iy = this->YI(node);
-        unsigned int iy_dist = floor(dist / IYSTEP) + 1;
+        float iy_step = (this->VMAX - this->VMIN) / IYSIZE;
+        unsigned int iy_dist = floor(dist / iy_step) + 1;
         unsigned int i = 0; // vector index
         unsigned int j = 0; // array index
         unsigned int jmin = 0; // minimal j index