From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Fri, 24 Apr 2020 12:59:53 +0000 (+0200)
Subject: Consider only one goal in goal found procedure
X-Git-Tag: v0.4.0~4^2
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/d577dfe96aefce54791a3f1e4bc68e606669c76a

Consider only one goal in goal found procedure
---

diff --git a/src/rrts.cc b/src/rrts.cc
index d4ea90e..c48e680 100644
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -2,7 +2,7 @@
 #include "rrts.h"
 
 #include "reeds_shepp.h"
-
+#include <iostream>
 template <typename T> int sgn(T val) {
         return (T(0) < val) - (val < T(0));
 }
@@ -225,19 +225,18 @@ void RRTS::join_steered(RRTNode *f)
 bool RRTS::goal_found(RRTNode &f)
 {
         bool found = false;
-        for (auto &g: this->goals()) {
-                double cost = this->cost_build(f, g);
-                double edist = sqrt(
-                        pow(f.x() - g.x(), 2)
-                        + pow(f.y() - g.y(), 2)
-                );
-                double adist = std::abs(f.h() - g.h());
-                if (edist < 0.05 && adist < M_PI / 32) {
-                        found = true;
-                        if (g.p() == nullptr || cc(f) + cost < cc(g)) {
-                                g.p(&f);
-                                g.c(cost);
-                        }
+        auto &g = this->goals().front();
+        double cost = this->cost_build(f, g);
+        double edist = sqrt(
+                pow(f.x() - g.x(), 2)
+                + pow(f.y() - g.y(), 2)
+        );
+        double adist = std::abs(f.h() - g.h());
+        if (edist < 0.05 && adist < M_PI / 32) {
+                found = true;
+                if (g.p() == nullptr || cc(f) + cost < cc(g)) {
+                        g.p(&f);
+                        g.c(cost);
                 }
         }
         return found;