From f11f592399aa2d1ec0d17996badd82430e42c577 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 3 May 2021 15:03:02 +0200
Subject: [PATCH] Use goal zone in connect

---
 src/rrts.cc | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

diff --git a/src/rrts.cc b/src/rrts.cc
index 5a69bae..438392d 100644
--- 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();
-- 
2.39.2