Implement grid based nv

diff --git a/src/rrtext4.cc b/src/rrtext4.cc
index d2f5b0022ca8870d1032f7a9c8d150879fb115c3..33f0042b8470adbb2c9235b09cadc3f10fe2cf5b 100644 (file)
--- a/src/rrtext4.cc
+++ b/src/rrtext4.cc
@@ -94,5 +94,12 @@ RRTNode *RRTExt4::nn(RRTNode &t)
 std::vector<RRTNode *> RRTExt4::nv(RRTNode &t)
 {
         std::vector<RRTNode *> nv;
+        unsigned int txi = this->xi(t);
+        unsigned int tyi = this->yi(t);
+        unsigned int l = 0;
+        double cost = std::min(GAMMA(this->nodes().size()), ETA);
+        for (auto f: this->grid_[txi][tyi].nodes())
+                if (this->cost_search(*f, t) < cost)
+                        nv.push_back(f);
         return nv;
 }