Add detection of node in front of node

diff --git a/base/rrtnode.cc b/base/rrtnode.cc
index 59d5d4a07852ae225c5e2babb976b41efa135a4a..11ec99c87f6bc9b9a11e0b6b830e4340e7f2be28 100644 (file)
--- a/base/rrtnode.cc
+++ b/base/rrtnode.cc
@@ -254,6 +254,18 @@ bool RRTNode::visit()
         return false;
 }
 
+bool RRTNode::inFront(RRTNode *n)
+{
+        float nx = cos(this->h());
+        float ny = sin(this->h());
+        float dx = n->x() - this->x();
+        float dy = n->y() - this->y();
+        float dot = nx * dx + ny * dy;
+        if (dot > 0)
+                return true;
+        return false;
+}
+
 RRTEdge::RRTEdge():
         init_(new RRTNode()),
         goal_(new RRTNode())

diff --git a/incl/rrtnode.h b/incl/rrtnode.h
index 4a501bfa8b804869c73192110fc3a3bdb8b435e1..24fb4b40d6295359aa80a793fb6ee8fc91d3163e 100644 (file)
--- a/incl/rrtnode.h
+++ b/incl/rrtnode.h
@@ -109,6 +109,7 @@ class RRTNode {
                 static bool comp_ccost(RRTNode *n1, RRTNode *n2);
                 float update_ccost();
                 bool visit();
+                bool inFront(RRTNode *n);
 };
 
 class RRTEdge {