From: Jiri Hubacek <hubacji1@fel.cvut.cz>
Date: Mon, 24 Sep 2018 08:00:38 +0000 (+0200)
Subject: Add IS_NEAR macro
X-Git-Tag: v0.2.0~28
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/4e414b2ea56bdb2b74d45bcab4eb3839d41a8a9f

Add IS_NEAR macro
---

diff --git a/incl/rrtnode.h b/incl/rrtnode.h
index ef66143..40ca962 100644
--- a/incl/rrtnode.h
+++ b/incl/rrtnode.h
@@ -20,6 +20,12 @@ 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; })
+
 class RRTNode {
         private:
                 float dcost_ = 0; // direct cost (of edge) to parent