Remove unused macros

[hubacji1/iamcar.git] / base / rrtbase.cc

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 8785ad7230849e6b3f736d5d6df3610f092892d1..5bb1385616c49b3b0826a3409bf8ae303338524f 100644 (file)
--- 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