From e3baff97c4b756ef3b5de18b015c0a11c6ab0988 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Fri, 14 Jun 2019 14:47:35 +0200
Subject: [PATCH] Add detection of node in front of node

---
 base/rrtnode.cc | 12 ++++++++++++
 incl/rrtnode.h  |  1 +
 2 files changed, 13 insertions(+)

diff --git a/base/rrtnode.cc b/base/rrtnode.cc
index 59d5d4a..11ec99c 100644
--- 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 4a501bf..24fb4b4 100644
--- 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 {
-- 
2.39.2