Merge branch 'feature/refactor-nn'

[hubacji1/iamcar.git] / decision_control / rrtplanner.cc

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index cdaca08a1cce42e46b4e36d9baf08ea0387b045e..59bdf96b9251eb9a8f8fa5413130e306ca7d04ef 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -19,9 +19,6 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #include <cstdlib>
 #include <ctime>
 #include "compile.h"
-#include "nn.h"
-#include "nv.h"
-#include "sample.h"
 #include "steer.h"
 #include "rrtplanner.h"
 #include "cost.h"
@@ -62,7 +59,7 @@ bool LaValle1998::next()
                         if (this->collide(pn, ns)) {
                                 pn->children().pop_back();
                                 ns->remove_parent();
-                                this->iy_[IYI(ns->y())].pop_back();
+                                this->iy_[this->YI(ns)].pop_back();
                                 en_add = false;
                         } else {
                                 this->ocost(ns);
@@ -118,7 +115,7 @@ bool Kuwata2008::next()
                         if (this->collide(pn, ns)) {
                                 pn->children().pop_back();
                                 ns->remove_parent();
-                                this->iy_[IYI(ns->y())].pop_back();
+                                this->iy_[this->YI(ns)].pop_back();
                                 en_add = false;
                         } else {
                                 this->ocost(ns);
@@ -146,7 +143,7 @@ bool Kuwata2008::next()
                                 if (this->collide(pn, ns)) {
                                         pn->children().pop_back();
                                         ns->remove_parent();
-                                        this->iy_[IYI(ns->y())].pop_back();
+                                        this->iy_[this->YI(ns)].pop_back();
                                         en_add = false;
                                 } else {
                                         this->ocost(ns);
@@ -214,7 +211,7 @@ bool Karaman2011::next()
                         this->add_iy(ns);
                         // connect
                         if (!this->connect(pn, ns, nvs)) {
-                                this->iy_[IYI(ns->y())].pop_back();
+                                this->iy_[this->YI(ns)].pop_back();
                                 en_add = false;
                         } else {
                                 // rewire
@@ -436,7 +433,7 @@ bool T2::next()
                         this->add_iy(ns);
                         // connect
                         if (!this->connect(pn, ns, nvs)) {
-                                this->iy_[IYI(ns->y())].pop_back();
+                                this->iy_[this->YI(ns)].pop_back();
                                 this->nodes().pop_back();
                                 delete ns;
                                 en_add = false;
@@ -478,7 +475,7 @@ bool T2::next()
                                 if (this->collide(pn, ns)) {
                                         pn->children().pop_back();
                                         ns->remove_parent();
-                                        this->iy_[IYI(ns->y())].pop_back();
+                                        this->iy_[this->YI(ns)].pop_back();
                                         this->nodes().pop_back();
                                         delete ns;
                                         en_add = false;
@@ -518,7 +515,7 @@ bool T2::next()
                                 if (this->collide(pn, ns)) {
                                         pn->children().pop_back();
                                         ns->remove_parent();
-                                        this->iy_[IYI(ns->y())].pop_back();
+                                        this->iy_[this->YI(ns)].pop_back();
                                         this->nodes().pop_back();
                                         delete ns;
                                         en_add = false;
@@ -779,7 +776,7 @@ int Klemm2015::extendstar1(RRTNode *rs, RRTNode **xn)
        this->add_iy(ns);
        // connect
        if (!this->connect(nn, ns, nvs)) {
-               this->iy_[IYI(ns->y())].pop_back();
+               this->iy_[this->YI(ns)].pop_back();
                 ret = 2;
        } else {
                 // rewire
@@ -820,7 +817,7 @@ int Klemm2015::extendstarC(RRTNode *rs)
                         this->add_iy(ns);
                         // connect
                         if (!this->connect(pn, ns, nvs)) {
-                                this->iy_[IYI(ns->y())].pop_back();
+                                this->iy_[this->YI(ns)].pop_back();
                                 en_add = false;
                                 ret = 2;
                         } else {