Use euclid, angle diff. by default for goal found

diff --git a/src/rrts.cc b/src/rrts.cc
index b62a965fb2135a9ebe3bd475210e219a9ec1790e..f97c66773c45587d88b1c3ae911993f44ef9312b 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -226,11 +226,12 @@ bool RRTS::goal_found(RRTNode &f)
 {
         auto &g = this->goals().front();
         double cost = this->cost_build(f, g);
-        if (g.drivable(f)) {
-                this->steer(f, g);
-                if (std::get<0>(this->collide_steered_from(f)))
-                        return false;
-                this->join_steered(&f);
+        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) {
                 if (g.p() == nullptr || cc(f) + cost < cc(g)) {
                         g.p(&f);
                         g.c(cost);