Consider only one goal in path procedure

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

diff --git a/src/rrts.cc b/src/rrts.cc
index 2010c1c4c1895364a7beb4b75d6f8f759ed636fc..d4ea90e7eb8caf5b1e923cafc4fbabb475366fc6 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -303,14 +303,6 @@ std::vector<RRTNode *> RRTS::path()
         if (this->goals().size() == 0)
                 return path;
         RRTNode *goal = &this->goals().front();
-        for (auto &n: this->goals()) {
-                if (
-                        n.p() != nullptr
-                        && (n.c() < goal->c() || goal->p() == nullptr)
-                ) {
-                        goal = &n;
-                }
-        }
         if (goal->p() == nullptr)
                 return path;
         while (goal != nullptr) {