F 179e12c

diff --git a/src/rrts.cc b/src/rrts.cc
index aa37552146b94abd2130473b4eaa5f8d5dbf2912..daa2c9d2ff7812313ef1eefdf308f758fa31f8d6 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -44,7 +44,7 @@ double RRTS::elapsed()
 
 void RRTS::log_path_cost()
 {
-        this->log_path_cost_.push_back(cc(this->goals().front()));
+        this->log_path_cost_.push_back(this->goals().front().cc);
         this->log_path_iter_ += 20;
 }
 
@@ -385,7 +385,7 @@ bool RRTS::goal_found(RRTNode &f)
         );
         double adist = std::abs(f.h() - g.h());
         if (edist < 0.05 && adist < M_PI / 32) {
-                if (g.p() == nullptr || cc(f) + cost < cc(g)) {
+                if (g.p() == nullptr || f.cc + cost < g.cc) {
                         g.p(&f);
                         g.c(cost);
                 }
@@ -399,14 +399,14 @@ bool RRTS::connect()
 {
         RRTNode *t = &this->steered().front();
         RRTNode *f = this->nn(this->samples().back());
-        double cost = cc(*f) + this->cost_build(*f, *t);
+        double cost = f->cc + this->cost_build(*f, *t);
         for (auto n: this->nv(*t)) {
                 if (
                         !std::get<0>(this->collide_two_nodes(*n, *t))
-                        && cc(*n) + this->cost_build(*n, *t) < cost
+                        && n->cc + this->cost_build(*n, *t) < cost
                 ) {
                         f = n;
-                        cost = cc(*n) + this->cost_build(*n, *t);
+                        cost = n->cc + this->cost_build(*n, *t);
                 }
         }
         // steer from f->t and then continue with the steered.
@@ -443,7 +443,7 @@ void RRTS::rewire()
         for (auto n: this->nv(*f)) {
                 if (
                         !std::get<0>(this->collide_two_nodes(*f, *n))
-                        && cc(*f) + this->cost_build(*f, *n) < cc(*n)
+                        && f->cc + this->cost_build(*f, *n) < n->cc
                 ) {
                         this->tmp_steer(*f, *n);
                         if (this->tmp_steered_.size() > 0) {
@@ -626,7 +626,7 @@ Json::Value RRTS::json()
         }
         {
                 if (this->path().size() > 0) {
-                        jvo["cost"] = cc(*this->path().back());
+                        jvo["cost"] = this->path().back()->cc;
                         jvo["entry"][0] = this->goals().front().x();
                         jvo["entry"][1] = this->goals().front().y();
                         jvo["entry"][2] = this->goals().front().h();