Add nn4 procedure

Add nearest neighbour procedure with modifying node's heading when
comparing to others nodes. Node's heading is set to comparing node's
heading.

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 2568de0c67573d487aea6c7ba9105fcfb35e92e2..44b2050733249bbed68b76cdaf537f05fda93dea 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -22,6 +22,7 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Test planner `T2` based on RRT\* [Karaman2011] with steering from newly added
   nodes to goal.
 - OpenGL 2.1 plot using SDL2.
+- `nn4` procedure - set node's heading to heading of currently comparing node.
 
 ### Changed
 - Build with Ninja.

diff --git a/base/nn.cc b/base/nn.cc
index 8098cee5689bbdafcc7f13b3223ab6a05bfc1c4a..c9aea72a65dc13d2e19ab4077d2edbdde003099e 100644 (file)
--- a/base/nn.cc
+++ b/base/nn.cc
@@ -111,3 +111,45 @@ RRTNode *nn3(
         }
         return nn.nn;
 }
+
+RRTNode *nn4(
+                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++) {
+                        node->h(nodes[iyj][i]->h());
+                        if ((*cost)(nodes[iyj][i], node) < nn.mc) {
+                                nn.mc = (*cost)(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++) {
+                                node->h(nodes[iyj][i]->h());
+                                if ((*cost)(nodes[iyj][i], node) < nn.mc) {
+                                        nn.mc = (*cost)(nodes[iyj][i], node);
+                                        nn.nn = nodes[iyj][i];
+                                }
+                        }
+                }
+                j++;
+        }
+        return nn.nn;
+}

diff --git a/incl/nn.h b/incl/nn.h
index eaf4447e5e52326a21a78e276c4b0fd630b1144a..719f793cb38aef5a475ccd618bf91d75804913a8 100644 (file)
--- a/incl/nn.h
+++ b/incl/nn.h
@@ -43,5 +43,9 @@ RRTNode *nn3(
                 std::vector<RRTNode *> (&nodes)[IYSIZE],
                 RRTNode *node,
                 float (*cost)(RRTNode *, RRTNode *));
+RRTNode *nn4(
+                std::vector<RRTNode *> (&nodes)[IYSIZE],
+                RRTNode *node,
+                float (*cost)(RRTNode *, RRTNode *));
 
 #endif