Fix indexing overflow

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 97e4a5f1d354eff8d72141dfdef8b653c5e2f99d..563fc04d8364c3c99da74f1f2646b750985bc581 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -1046,6 +1046,8 @@ int RRTBase::XI(RRTNode *n)
 {
         float step = (this->HMAX - this->HMIN) / IXSIZE;
         float index = (int) (floor(n->x() - this->HMIN) / step);
+        if (index < 0) index = 0;
+        if (index >= IXSIZE) index = IXSIZE - 1;
         return index;
 }
 
@@ -1053,6 +1055,8 @@ int RRTBase::YI(RRTNode *n)
 {
         float step = (this->VMAX - this->VMIN) / IYSIZE;
         float index = (int) (floor(n->y() - this->VMIN) / step);
+        if (index < 0) index = 0;
+        if (index >= IYSIZE) index = IYSIZE - 1;
         return index;
 }