Merge branch 'fix-path-opt-ext13'

[hubacji1/iamcar2.git] / rrts / src / rrts.cc

diff --git a/rrts/src/rrts.cc b/rrts/src/rrts.cc
index 39b351687cb0a3d70b24c308ce867dd70a64ed01..5a142174de1babc904991b1b15d0b680cd29e60f 100644 (file)
--- a/rrts/src/rrts.cc
+++ b/rrts/src/rrts.cc
@@ -55,10 +55,10 @@ RRTNode::p() const
 }
 
 void
-RRTNode::p(RRTNode& p, bool connecting_goal)
+RRTNode::p(RRTNode& p, bool can_be_too_close)
 {
        assert(this != &p);
-       if (!connecting_goal) {
+       if (!can_be_too_close) {
                assert(!(std::abs(p.x() - this->x()) < 1e-3
                        && std::abs(p.y() - this->y()) < 1e-3
                        && std::abs(p.h() - this->h()) < 1e-3));
@@ -149,18 +149,16 @@ RRTNode::operator==(RRTNode const& n)
 void
 RRTS::recompute_cc_for_predecessors_and(RRTNode* g)
 {
-       assert(this->_path.size() == 0);
+       std::vector<RRTNode*> path;
+       path.reserve(this->_path.size());
        while (g != nullptr) {
-               this->_path.push_back(g);
+               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]));
+       std::reverse(path.begin(), path.end());
+       for (unsigned int i = 1; i < path.size(); i++) {
+               path[i]->c(this->cost_build(*path[i - 1], *path[i]));
        }
-       this->_path.clear();
 }
 
 void