Merge branch 'release/0.7.0'

[hubacji1/iamcar.git] / incl / rrtnode.h

diff --git a/incl/rrtnode.h b/incl/rrtnode.h
index 4ea4adfcfb57032d2a0852d29b50c8fb7247735f..a515386044d94aa3f320b145c8b0c5cca56c0893 100644 (file)
--- a/incl/rrtnode.h
+++ b/incl/rrtnode.h
@@ -20,11 +20,21 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 
 #include <vector>
 
-#define IS_NEAR(a, b) ({ __typeof__ (a) _a = (a); \
-                __typeof__ (b) _b = (b); \
-                pow(pow(_b->x() - _a->x(), 2) + \
-                                pow(_b->y() - _a->y(), 2), 0.5) < 0.2 && \
-                std::abs(_b->h() - _a->h()) < M_PI / 32 ? true : false; })
+#define IS_NEAR(a, b) ({ \
+        __typeof__ (a) _a = (a); \
+        __typeof__ (b) _b = (b); \
+        pow(pow(_b->x() - _a->x(), 2) \
+        + pow(_b->y() - _a->y(), 2), 0.5) < 0.2 \
+        && std::abs(_b->h() - _a->h()) < M_PI / 32 ? true : false; \
+})
+
+#define GOAL_IS_NEAR(a, b) ({ \
+        __typeof__ (a) _a = (a); \
+        __typeof__ (b) _b = (b); \
+        pow(pow(_b->x() - _a->x(), 2) \
+        + pow(_b->y() - _a->y(), 2), 0.5) < 0.05 \
+        && std::abs(_b->h() - _a->h()) < M_PI / 32 ? true : false; \
+})
 
 template <typename T> int sgn(T val) {
             return (T(0) < val) - (val < T(0));
@@ -99,6 +109,11 @@ class RRTNode {
                 static bool comp_ccost(RRTNode *n1, RRTNode *n2);
                 float update_ccost();
                 bool visit();
+                /** Return ``true`` if ``n`` is in front of ``this``.
+                *
+                * @param n The node that is beeing checked.
+                */
+                bool inFront(RRTNode *n);
 };
 
 class RRTEdge {