Use goal zone in connect

diff --git a/src/rrts.cc b/src/rrts.cc
index 5a69baec5749f73b65227b3982ba09087b4efb05..438392dabe1a54e4fe020ba30e8591cffa626b53 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -418,11 +418,16 @@ bool RRTS::connect()
                 this->join_tmp_steered(f);
                 f = &this->nodes().back();
         }
-        if (sqrt(
-                pow(f->x() - t->x(), 2)
-                + pow(f->y() - t->y(), 2)
-        ) > 0.2)
-                return false;
+        auto fbc = BicycleCar();
+        fbc.x(f->x());
+        fbc.y(f->y());
+        fbc.h(f->h());
+        auto tbc = BicycleCar();
+        tbc.x(t->x());
+        tbc.y(t->y());
+        tbc.h(t->h());
+        if (!tbc.drivable(fbc))
+            return false;
         // cont.
         this->store_node(this->steered().front());
         t = &this->nodes().back();