Add goal zone extension

diff --git a/api/rrtext.h b/api/rrtext.h
index 18d1f46a096af689847421b8fd2a6b2b2c612fdd..46d7e40c3ad76175d28877fd7f37448474b773ac 100644 (file)
--- 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 c9205b852a40dedf49b96add7e0b07c91744c3ae..71fe9528ae4cc604d853f6825b60f61ea0ac7c97 100644 (file)
--- 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 (file)
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;
+}