Use search cost in nn, nv only

diff --git a/src/rrts.cc b/src/rrts.cc
index 868f4712fafc370b924650a9adc0810c1899b2ed..bc433335dba99098645f16d122c2551835b4261b 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -399,14 +399,14 @@ bool RRTS::connect()
 {
         RRTNode *t = &this->steered().front();
         RRTNode *f = this->nn(this->samples().back());
-        double cost = this->cost_search(*f, *t);
+        double cost = this->cost_build(*f, *t);
         for (auto n: this->nv(*t)) {
                 if (
                         !std::get<0>(this->collide_two_nodes(*n, *t))
-                        && this->cost_search(*n, *t) < cost
+                        && this->cost_build(*n, *t) < cost
                 ) {
                         f = n;
-                        cost = this->cost_search(*n, *t);
+                        cost = this->cost_build(*n, *t);
                 }
         }
         // steer from f->t and then continue with the steered.
@@ -443,8 +443,16 @@ void RRTS::rewire()
         for (auto n: this->nv(*f)) {
                 if (
                         !std::get<0>(this->collide_two_nodes(*f, *n))
-                        && cc(*f) + this->cost_search(*f, *n) < cc(*n)
+                        && cc(*f) + this->cost_build(*f, *n) < cc(*n)
                 ) {
+                        this->tmp_steer(*f, *n);
+                        if (this->tmp_steered_.size() > 0) {
+                                auto col = this->collide_tmp_steered_from(*f);
+                                if (std::get<0>(col))
+                                        continue;
+                                this->join_tmp_steered(f);
+                                f = &this->nodes().back();
+                        }
                         n->p(f);
                         n->c(this->cost_build(*f, *n));
                 }