Use splitted costs

diff --git a/src/rrts.cc b/src/rrts.cc
index 5c2460888156a9b100eecd5f5af5b723739fda76..4458d32108c5877ddeb648a579df7248a21fc17f 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -96,11 +96,11 @@ void RRTS::sample()
 RRTNode *RRTS::nn(RRTNode &t)
 {
         RRTNode *nn = &this->nodes().front();
-        double cost = this->cost(*nn, t);
+        double cost = this->cost_search(*nn, t);
         for (auto &f: this->nodes()) {
-                if (this->cost(f, t) < cost) {
+                if (this->cost_search(f, t) < cost) {
                         nn = &f;
-                        cost = this->cost(f, t);
+                        cost = this->cost_search(f, t);
                 }
         }
         return nn;
@@ -111,7 +111,7 @@ std::vector<RRTNode *> RRTS::nv(RRTNode &t)
         std::vector<RRTNode *> nv;
         double cost = std::min(GAMMA(this->nodes().size()), ETA);
         for (auto &f: this->nodes())
-                if (this->cost(f, t) < cost)
+                if (this->cost_search(f, t) < cost)
                         nv.push_back(&f);
         return nv;
 }
@@ -141,7 +141,7 @@ void RRTS::join_steered(RRTNode *f)
                 this->nodes().push_back(this->steered().front());
                 RRTNode *t = &this->nodes().back();
                 t->p(f);
-                t->c(this->cost(*f, *t));
+                t->c(this->cost_build(*f, *t));
                 this->steered().erase(this->steered().begin());
                 f = t;
         }
@@ -151,7 +151,7 @@ bool RRTS::goal_found(RRTNode &f)
 {
         bool found = false;
         for (auto &g: this->goals()) {
-                double cost = this->cost(f, g);
+                double cost = this->cost_build(f, g);
                 double edist = sqrt(
                         pow(f.x() - g.x(), 2)
                         + pow(f.y() - g.y(), 2)
@@ -174,20 +174,20 @@ bool RRTS::connect()
         bool conn = false;
         RRTNode *t = &this->steered().front();
         RRTNode *f = this->nn(this->samples().back());
-        double cost = this->cost(*f, *t);
+        double cost = this->cost_search(*f, *t);
         for (auto n: this->nv(*t)) {
                 if (
                         !std::get<0>(this->collide_two_nodes(*n, *t))
-                        && this->cost(*n, *t) < cost
+                        && this->cost_search(*n, *t) < cost
                 ) {
                         f = n;
-                        cost = this->cost(*n, *t);
+                        cost = this->cost_search(*n, *t);
                 }
         }
         this->nodes().push_back(this->steered().front());
         t = &this->nodes().back();
         t->p(f);
-        t->c(this->cost(*f, *t));
+        t->c(this->cost_build(*f, *t));
         conn = true;
         return conn;
 }
@@ -198,9 +198,11 @@ void RRTS::rewire()
         for (auto n: this->nv(*f)) {
                 if (
                         !std::get<0>(this->collide_two_nodes(*f, *n))
-                        && cc(*f) + this->cost(*f, *n) < cc(*n)
-                )
+                        && cc(*f) + this->cost_search(*f, *n) < cc(*n)
+                ) {
                         n->p(f);
+                        n->c(this->cost_build(*f, *n));
+                }
         }
 }