Use nn in RRTBase

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 7a7c194d3e7df5519b2cd4cb223bc037cce30745..41197b301a6732c0632f8ac67fccb8c47fe8dec5 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -1057,8 +1057,39 @@ float RRTBase::cost(RRTNode *init, RRTNode *goal)
 
 RRTNode *RRTBase::nn(RRTNode *rs)
 {
-        return nn4(this->iy_, rs, nullptr);
-        //return nn3(this->iy_, rs, nullptr);
+        int iy = IYI(rs->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 < this->iy_[iyj].size(); i++) {
+                        if (EDIST(this->iy_[iyj][i], rs) < nn.mc) {
+                                nn.mc = EDIST(this->iy_[iyj][i], rs);
+                                nn.nn = this->iy_[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 < this->iy_[iyj].size(); i++) {
+                                if (EDIST(this->iy_[iyj][i], rs) < nn.mc) {
+                                        nn.mc = EDIST(this->iy_[iyj][i], rs);
+                                        nn.nn = this->iy_[iyj][i];
+                                }
+                        }
+                }
+                j++;
+        }
+        return nn.nn;
 }
 
 std::vector<RRTNode *> RRTBase::nv(RRTNode *node, float dist)