From 5b2f1bec8c615bcadff2c83615de4bb0e566c773 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 26 May 2020 15:14:05 +0200
Subject: [PATCH] Add goal zone extension

---
 api/rrtext.h    |  8 ++++++++
 api/rrts.h      |  2 +-
 src/rrtext11.cc | 19 +++++++++++++++++++
 3 files changed, 28 insertions(+), 1 deletion(-)
 create mode 100644 src/rrtext11.cc

diff --git a/api/rrtext.h b/api/rrtext.h
index 18d1f46..46d7e40 100644
--- a/api/rrtext.h
+++ b/api/rrtext.h
@@ -16,6 +16,14 @@
 // ext9
 #define GRID_MAX_HI 60
 
+/*! \brief Goal Zone.
+
+*/
+class RRTExt11 : public virtual RRTS {
+        protected:
+                bool goal_found(RRTNode &f);
+};
+
 /*! \brief Different costs extension.
 
 Use different cost for bulding tree data structure and searching in the
diff --git a/api/rrts.h b/api/rrts.h
index c9205b8..71fe952 100644
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -138,7 +138,7 @@ class RRTS {
                 \param f RRT node to join steered nodes to.
                 */
                 void join_steered(RRTNode *f);
-                bool goal_found(RRTNode &f);
+                virtual bool goal_found(RRTNode &f);
                 // RRT* procedures
                 bool connect();
                 void rewire();
diff --git a/src/rrtext11.cc b/src/rrtext11.cc
new file mode 100644
index 0000000..32902ce
--- /dev/null
+++ b/src/rrtext11.cc
@@ -0,0 +1,19 @@
+#include "rrtext.h"
+
+bool RRTExt11::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);
+                if (g.p() == nullptr || cc(f) + cost < cc(g)) {
+                        g.p(&f);
+                        g.c(cost);
+                }
+                return true;
+        }
+        return false;
+}
-- 
2.39.2