From 1450652cd25e0374a59d5d0c7d419622a2c2b3f5 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 2 Apr 2019 16:59:25 +0200
Subject: [PATCH] Add nn using Matej's heuristic cost

---
 base/nn.cc | 40 ++++++++++++++++++++++++++++++++++++++++
 incl/nn.h  |  4 ++++
 2 files changed, 44 insertions(+)

diff --git a/base/nn.cc b/base/nn.cc
index 1ec352a..d076d04 100644
--- 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 0fc7e98..405cd8c 100644
--- 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
-- 
2.39.2