Add recompute cc method

diff --git a/incl/rrts.hh b/incl/rrts.hh
index 9c847ed0b075addf807dd06ce9b571eae6a4935b..a61558d10a2874bb3cb79c124c4a276e8b8f971f 100644 (file)
--- a/incl/rrts.hh
+++ b/incl/rrts.hh
@@ -72,6 +72,7 @@ protected:
        double time_ = 0.0;
        double last_goal_cc_ = 0.0;
        std::vector<RRTNode> last_path_;
+       void recompute_cc(RRTNode* g);
        void recompute_path_cc();
        double min_gamma_eta() const;
        bool should_continue() const;

diff --git a/src/rrtext13.cc b/src/rrtext13.cc
index f6dd77473d014baa673fe3042ad1ab4222bdd634..33e97318b121245ce4ba46683ecb1f35274a64c2 100644 (file)
--- a/src/rrtext13.cc
+++ b/src/rrtext13.cc
@@ -192,13 +192,14 @@ RRTExt13::dijkstra_backward()
                                this->join_steered(&f);
                                t.p(this->nodes_.back());
                                t.c(this->cost_build(this->nodes_.back(), t));
-                               this->recompute_path_cc();
+                               this->recompute_cc(&t);
                                if (!this->dn_[i].vi()) {
                                        pq.push(this->dn_[i]);
                                }
                        }
                }
        }
+       this->recompute_path_cc();
 }
 
 void

diff --git a/src/rrts.cc b/src/rrts.cc
index 191bf7fbfd7f3449c2ea53297946419d4ced6402..d2c93180f99d2000e4c07d156e88520d5cffb910 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -60,10 +60,9 @@ RRTNode::operator==(RRTNode const& n)
 }
 
 void
-RRTS::recompute_path_cc()
+RRTS::recompute_cc(RRTNode* g)
 {
        this->path_.clear();
-       RRTNode* g = &this->goal_;
        while (g != nullptr) {
                this->path_.push_back(g);
                g = g->p();
@@ -75,6 +74,12 @@ RRTS::recompute_path_cc()
        }
 }
 
+void
+RRTS::recompute_path_cc()
+{
+       this->recompute_cc(&this->goal_);
+}
+
 double
 RRTS::min_gamma_eta() const
 {