Use Euclidean distance in `nn4`

diff --git a/base/nn.cc b/base/nn.cc
index aa9eeaf9296fd7065b466f2c48aead86f8423f2b..8a937eaadefd0bc6e23212a14e5d811cb5964a52 100644 (file)
--- 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);