Add recompute path cc method

diff --git a/incl/rrts.hh b/incl/rrts.hh
index a9ef93c7351a233aea7b288a48c22104f9705ae1..3bfd07e83460c353a20cd8c2576ca9b096135ef3 100644 (file)
--- a/incl/rrts.hh
+++ b/incl/rrts.hh
@@ -71,6 +71,7 @@ protected:
        double eta_ = 0.5;
        double time_ = 0.0;
        double last_goal_cc_ = 0.0;
+       void recompute_path_cc();
        double min_gamma_eta() const;
        bool should_continue() const;
        void join_steered(RRTNode* f);

diff --git a/src/rrts.cc b/src/rrts.cc
index 029997e1c73173a702f1bc9712fc698ddd923d03..b0cfe222efa5be7d32cba618945e14a7593fdb86 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -59,6 +59,22 @@ RRTNode::operator==(RRTNode const& n)
        return this == &n;
 }
 
+void
+RRTS::recompute_path_cc()
+{
+       this->path_.clear();
+       RRTNode* g = &this->goal_;
+       while (g != nullptr) {
+               this->path_.push_back(g);
+               g = g->p();
+       }
+       std::reverse(this->path_.begin(), this->path_.end());
+       for (unsigned int i = 1; i < this->path_.size(); i++) {
+               this->path_[i]->c(this->cost_build(*this->path_[i - 1],
+                       *this->path_[i]));
+       }
+}
+
 double
 RRTS::min_gamma_eta() const
 {