Add nn3 procedure

diff --git a/CHANGELOG.md b/CHANGELOG.md
index e4d85dc28d000ff1350e00d9c96a383b49392022..fc29e1c8b4d0976fd5bf8c4a95a528cecf1ae220 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,7 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Compilation macros that can specify output binary parameters.
 - Auxiliary build and test scripts.
 - Nearest neighbour `nn2` procedure based on linear search over `nodes()`.
+- Nearest neighbour `nn3` procedure based on indexing over `y` axis.
 
 ## 0.1.0 - 2018-07-05
 ### Added

diff --git a/base/nn.cc b/base/nn.cc
index daa4c2c9845ee133d798a943e7657d7b1aea957a..cf38cf9aeb70307f83f12fa26e031d9f38cd9e40 100644 (file)
--- a/base/nn.cc
+++ b/base/nn.cc
@@ -67,3 +67,40 @@ RRTNode *nn2(
         }
         return nn;
 }
+
+RRTNode *nn3(
+                std::vector<RRTNode *> (&nodes)[IYSIZE],
+                RRTNode *node,
+                float (*cost)(RRTNode *, RRTNode *))
+{
+        int iy = floor(node->y() / IYSTEP) + floor(IYSIZE / 2);
+        RRTNode *nn = nullptr;
+        float mc = 9999;
+        unsigned int i = 0; // vector step
+        unsigned int j = 0; // array step
+        int iyj = 0;
+        while (mc > j * IYSTEP) {
+                iyj = (int) (iy + j);
+                if (iyj >= IYSIZE)
+                        iyj = IYSIZE - 1;
+                for (i = 0; i < nodes[iyj].size(); i++) {
+                        if ((*cost)(nodes[iyj][i], node) < mc) {
+                                mc = (*cost)(nodes[iyj][i], node);
+                                nn = nodes[iyj][i];
+                        }
+                }
+                if (j > 0) {
+                        iyj = (int) (iy - j);
+                        if (iyj < 0)
+                                iyj = 0;
+                        for (i = 0; i < nodes[iyj].size(); i++) {
+                                if ((*cost)(nodes[iyj][i], node) < mc) {
+                                        mc = (*cost)(nodes[iyj][i], node);
+                                        nn = nodes[iyj][i];
+                                }
+                        }
+                }
+                j++;
+        }
+        return nn;
+}

diff --git a/incl/nn.h b/incl/nn.h
index 41287ee6b71b6cd2fc4fc7d6fdbb4354c58e5909..54e84d7fd9b8df53080a84ef4b03c12727873c2f 100644 (file)
--- a/incl/nn.h
+++ b/incl/nn.h
@@ -18,6 +18,7 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #ifndef NEARESTNEIGHBOUR_H
 #define NEARESTNEIGHBOUR_H
 
+#include "rrtbase.h"
 #include "rrtnode.h"
 
 RRTNode *nn1(
@@ -28,5 +29,9 @@ RRTNode *nn2(
                 std::vector<RRTNode *> &nodes,
                 RRTNode *node,
                 float (*cost)(RRTNode *, RRTNode *));
+RRTNode *nn3(
+                std::vector<RRTNode *> (&nodes)[IYSIZE],
+                RRTNode *node,
+                float (*cost)(RRTNode *, RRTNode *));
 
 #endif