From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 31 May 2021 14:39:38 +0000 (+0200)
Subject: Use search cost in nn, nv only
X-Git-Tag: v0.7.0~8
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/621bd97d4b7c1dec3cd05ed61f0d9a83beccecd2

Use search cost in nn, nv only
---

diff --git a/src/rrts.cc b/src/rrts.cc
index 868f471..bc43333 100644
--- 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));
                 }