From: Jiri Hubacek <hubacji1@fel.cvut.cz>
Date: Fri, 19 Oct 2018 19:11:19 +0000 (+0200)
Subject: Use Euclidean distance in `nn4`
X-Git-Tag: v0.3.0~19
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/c15c76dc411d13620e4411887fded304a1274b95

Use Euclidean distance in `nn4`
---

diff --git a/base/nn.cc b/base/nn.cc
index aa9eeaf..8a937ea 100644
--- a/base/nn.cc
+++ b/base/nn.cc
@@ -112,6 +112,11 @@ RRTNode *nn3(
         return nn.nn;
 }
 
+#define EDIST(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); })
+
 RRTNode *nn4(
                 std::vector<RRTNode *> (&nodes)[IYSIZE],
                 RRTNode *node,
@@ -133,8 +138,8 @@ RRTNode *nn4(
                 for (i = 0; i < nodes[iyj].size(); i++) {
                         oh = node->h();
                         node->h(nodes[iyj][i]->h());
-                        if ((*cost)(nodes[iyj][i], node) < nn.mc) {
-                                nn.mc = (*cost)(nodes[iyj][i], node);
+                        if (EDIST(nodes[iyj][i], node) < nn.mc) {
+                                nn.mc = EDIST(nodes[iyj][i], node);
                                 nn.nn = nodes[iyj][i];
                         }
                         node->h(oh);
@@ -147,8 +152,8 @@ RRTNode *nn4(
                         for (i = 0; i < nodes[iyj].size(); i++) {
                                 oh = node->h();
                                 node->h(nodes[iyj][i]->h());
-                                if ((*cost)(nodes[iyj][i], node) < nn.mc) {
-                                        nn.mc = (*cost)(nodes[iyj][i], node);
+                                if (EDIST(nodes[iyj][i], node) < nn.mc) {
+                                        nn.mc = EDIST(nodes[iyj][i], node);
                                         nn.nn = nodes[iyj][i];
                                 }
                                 node->h(oh);