X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/blobdiff_plain/8f38e07029c9b74ac414c04a8aed19f83ae9c429..016beae2048a11c2d6888206795db8e3dfed3203:/base/rrtbase.cc

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 8785ad7..5bb1385 100644
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -1083,13 +1083,14 @@ float RRTBase::cost(RRTNode *init, RRTNode *goal)
 RRTNode *RRTBase::nn(RRTNode *rs)
 {
         int iy = this->YI(rs);
+        float iy_step = (this->VMAX - this->VMIN) / IYSIZE;
         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) {
+        while (nn.mc > j * iy_step) {
                 iyj = (int) (iy + j);
                 if (iyj >= IYSIZE)
                         iyj = IYSIZE - 1;
@@ -1121,7 +1122,8 @@ std::vector<RRTNode *> RRTBase::nv(RRTNode *node, float dist)
 {
         std::vector<RRTNode *> nvs;
         unsigned int iy = this->YI(node);
-        unsigned int iy_dist = floor(dist / IYSTEP) + 1;
+        float iy_step = (this->VMAX - this->VMIN) / IYSIZE;
+        unsigned int iy_dist = floor(dist / iy_step) + 1;
         unsigned int i = 0; // vector index
         unsigned int j = 0; // array index
         unsigned int jmin = 0; // minimal j index