Use indexing getters

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 9b7450d689974a19b4f5023210dc02f67b476411..09cc0f6eed02153e9d792b158a781efca56bb70e 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -330,7 +330,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 +341,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)
@@ -1001,7 +1001,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++) {
@@ -1067,7 +1067,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;
@@ -1105,7 +1105,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

diff --git a/decision_control/rrtplanner.cc b/decision_control/rrtplanner.cc
index cdaca08a1cce42e46b4e36d9baf08ea0387b045e..b019cfa6472c07fa2db5c61e1b72d47acc464915 100644 (file)
--- a/decision_control/rrtplanner.cc
+++ b/decision_control/rrtplanner.cc
@@ -62,7 +62,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 +118,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 +146,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 +214,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 +436,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 +478,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 +518,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 +779,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 +820,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 {