From 01bf4851f8582ff42c635c38ad00acb722b8b68a Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 24 Jun 2019 16:12:37 +0200 Subject: [PATCH] Use indexing getters --- base/rrtbase.cc | 12 ++++++------ decision_control/rrtplanner.cc | 18 +++++++++--------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/base/rrtbase.cc b/base/rrtbase.cc index 9b7450d..09cc0f6 100644 --- 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 RRTBase::nv(RRTNode *node, float dist) { std::vector 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 cdaca08..b019cfa 100644 --- 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 { -- 2.39.2