From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 24 Jun 2019 13:26:23 +0000 (+0200)
Subject: Use nn in RRTBase
X-Git-Tag: v0.7.0~5^2~12
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/58e9b178dbe80dc35c33545236ae55bb4d94b2f8

Use nn in RRTBase
---

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 7a7c194..41197b3 100644
--- 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)