Add nn using Matej's heuristic cost

diff --git a/base/nn.cc b/base/nn.cc
index 1ec352ac4f1c04398c9839e35830c7537e14348a..d076d04a367af086ff1f9bbc522b1a7a44ed0cf1 100644 (file)
--- a/base/nn.cc
+++ b/base/nn.cc
@@ -201,3 +201,43 @@ RRTNode *nn5(
         }
         return nn.nn;
 }
+
+RRTNode *nn6(
+                std::vector<RRTNode *> (&nodes)[IYSIZE],
+                RRTNode *node,
+                float (*cost)(RRTNode *, RRTNode *))
+{
+        int iy = IYI(node->y());
+        struct mcnn nn;
+        nn.nn = nullptr;
+        nn.mc = 9999;
+        unsigned int i = 0; // vector step
+        unsigned int j = 0; // array step
+        int iyj = 0;
+        while (nn.mc > j * IYSTEP) {
+                iyj = (int) (iy + j);
+                if (iyj >= IYSIZE)
+                        iyj = IYSIZE - 1;
+                #pragma omp parallel for reduction(minn: nn)
+                for (i = 0; i < nodes[iyj].size(); i++) {
+                        if (co4(nodes[iyj][i], node) < nn.mc) {
+                                nn.mc = co4(nodes[iyj][i], node);
+                                nn.nn = nodes[iyj][i];
+                        }
+                }
+                if (j > 0) {
+                        iyj = (int) (iy - j);
+                        if (iyj < 0)
+                                iyj = 0;
+                        #pragma omp parallel for reduction(minn: nn)
+                        for (i = 0; i < nodes[iyj].size(); i++) {
+                                if (co4(nodes[iyj][i], node) < nn.mc) {
+                                        nn.mc = co4(nodes[iyj][i], node);
+                                        nn.nn = nodes[iyj][i];
+                                }
+                        }
+                }
+                j++;
+        }
+        return nn.nn;
+}

diff --git a/incl/nn.h b/incl/nn.h
index 0fc7e982b2f580fe6a0ee0b83c802cdc8fb011c3..405cd8ca8633dc185d1fe8ca7cbf6b651dea6766 100644 (file)
--- a/incl/nn.h
+++ b/incl/nn.h
@@ -53,5 +53,9 @@ RRTNode *nn5(
                 RRTNode *node,
                 float (*cost)(RRTNode *, RRTNode *),
                 char tree);
+RRTNode *nn6(
+                std::vector<RRTNode *> (&nodes)[IYSIZE],
+                RRTNode *node,
+                float (*cost)(RRTNode *, RRTNode *));
 
 #endif