From ca2ecc344b9c4ca88d52dad78a51b1e67a3be3a6 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 14 Oct 2019 10:40:32 +0200 Subject: [PATCH] Implement grid based nn --- src/rrtext4.cc | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/src/rrtext4.cc b/src/rrtext4.cc index 8825c07..d2f5b00 100644 --- 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; } -- 2.39.2