Make `nv` procedure part of RRTBase

[hubacji1/iamcar.git] / base / rrtbase.cc

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 4725655e405c4077524b755f67a88fcba79b6c78..47465c8fafec72bc2e3648df3755ae96cfbad6d2 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -30,6 +30,7 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #include "cost.h"
 #include "steer.h"
 #include "nn.h"
+#include "nv.h"
 
 extern SDL_Window* gw;
 extern SDL_GLContext gc;
@@ -691,3 +692,28 @@ RRTNode *RRTBase::nn(RRTNode *rs)
 {
         return nn4(this->iy_, rs, nullptr);
 }
+
+std::vector<RRTNode *> RRTBase::nv(RRTNode *node, float dist)
+{
+        std::vector<RRTNode *> nvs;
+        unsigned int iy = IYI(node->y());
+        unsigned int iy_dist = floor(dist / IYSTEP) + 1;
+        unsigned int i = 0; // vector index
+        unsigned int j = 0; // array index
+        unsigned int jmin = 0; // minimal j index
+        unsigned int jmax = 0; // maximal j index
+        jmin = iy - iy_dist;
+        jmin = (jmin > 0) ? jmin : 0;
+        jmax = iy + iy_dist + 1;
+        jmax = (jmax < IYSIZE) ? jmax : IYSIZE;
+        #pragma omp parallel for reduction(merge: nvs)
+        for (j = jmin; j < jmax; j++) {
+                #pragma omp parallel for reduction(merge: nvs)
+                for (i = 0; i < this->iy_[j].size(); i++) {
+                        if (this->cost(this->iy_[j][i], node) < dist) {
+                                nvs.push_back(this->iy_[j][i]);
+                        }
+                }
+        }
+        return nvs;
+}