X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/blobdiff_plain/ececbcb87a5a9686c8da748aa2abe8dd53066832..2fa9de2da84b5b3b12152d55a93439e0ce28950e:/decision_control/rrtplanner.cc

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index cdaca08..59bdf96 100644
--- 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 {