Fix index computation, type

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 09cc0f6eed02153e9d792b158a781efca56bb70e..516504ef3a6fff99e4a81825acdbe469279ef75c 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -1034,14 +1034,18 @@ std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
         return nodes;
 }
 
-float RRTBase::XI(RRTNode *n)
+int RRTBase::XI(RRTNode *n)
 {
-       return n->x();
+        float step = (this->HMAX - this->HMIN) / IXSIZE;
+        float index = (int) (floor(n->x() - this->HMIN) / step);
+        return index;
 }
 
-float RRTBase::YI(RRTNode *n)
+int RRTBase::YI(RRTNode *n)
 {
-       return n->y();
+        float step = (this->VMAX - this->VMIN) / IYSIZE;
+        float index = (int) (floor(n->y() - this->VMIN) / step);
+        return index;
 }
 
 // RRT Framework

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 54557adf9980c487689df10b23783bc100f15208..0c61dbcf15338a0401877fb6f30fce5f742b3e79 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -163,8 +163,8 @@ class RRTBase {
                 bool rebase(RRTNode *nr);
                 std::vector<RRTNode *> findt();
                 std::vector<RRTNode *> findt(RRTNode *n);
-                float XI(RRTNode *n);
-                float YI(RRTNode *n);
+                int XI(RRTNode *n);
+                int YI(RRTNode *n);
 
                 // RRT Framework
                 void setSamplingInfo(SamplingInfo si);