Implement grid based nn

diff --git a/src/rrtext4.cc b/src/rrtext4.cc
index 8825c0749439f5a719b9c6a627d09a7f09f8c387..d2f5b0022ca8870d1032f7a9c8d150879fb115c3 100644 (file)
--- a/src/rrtext4.cc
+++ b/src/rrtext4.cc
@@ -58,6 +58,36 @@ void RRTExt4::store_node(RRTNode n)
 RRTNode *RRTExt4::nn(RRTNode &t)
 {
         RRTNode *nn = &this->nodes().front();
+        unsigned int txi = this->xi(t);
+        unsigned int tyi = this->yi(t);
+        unsigned int l = 0;
+        while (this->cost_search(*nn, t) > l * ETA) {
+                int xi_min = txi - l;
+                if (xi_min < 0)
+                        xi_min = 0;
+                int xi_max = txi + l;
+                if (xi_max > 99)
+                        xi_max = 99;
+                int yi_min = tyi - l;
+                if (yi_min < 0)
+                        yi_min = 0;
+                int yi_max = tyi + l;
+                if (yi_max > 99)
+                        yi_max = 99;
+                for (int xi = xi_min; xi <= xi_max; xi++) {
+                        this->grid_[xi][yi_max].nn(&t, nn, this);
+                }
+                for (int xi = xi_min; xi <= xi_max; xi++) {
+                        this->grid_[xi][yi_min].nn(&t, nn, this);
+                }
+                for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
+                        this->grid_[xi_min][yi].nn(&t, nn, this);
+                }
+                for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
+                        this->grid_[xi_max][yi].nn(&t, nn, this);
+                }
+                l++;
+        }
         return nn;
 }