Add structure for indexing by x, y axes to RRTBase

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 1038d9790cdf50ddc6a5b1bf29318ff920d2b2a0..59ef374efbb9698a490992e4d008380b84ef3111 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -268,6 +268,22 @@ bool RRTBase::add_iy(RRTNode *n)
         return true;
 }
 
+bool RRTBase::add_ixy(RRTNode *n)
+{
+        int ix = IXI(n->x());
+        if (ix < 0)
+                ix = 0;
+        if (ix >= IXSIZE)
+                ix = IXSIZE - 1;
+        int iy = IYI(n->y());
+        if (iy < 0)
+                iy = 0;
+        if (iy >= IYSIZE)
+                iy = IYSIZE - 1;
+        this->ixy_[ix][iy].add_node(n);
+        return true;
+}
+
 bool RRTBase::goal_found(bool f)
 {
         this->goal_found_ = f;

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index fc3362f9325a544d4a703cdf3d0221398dd81026..2799d84691f11cb2d629b4776eda9ac49570260a 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -86,6 +86,7 @@ class RRTBase {
                 std::vector<RRTNode *> &dnodes();
                 std::vector<RRTNode *> &samples();
                 std::vector<RRTNode *> iy_[IYSIZE];
+                Cell ixy_[IXSIZE][IYSIZE];
                 std::vector<CircleObstacle> *co();
                 std::vector<SegmentObstacle> *so();
                 std::vector<float> &clog();
@@ -108,6 +109,7 @@ class RRTBase {
                                 std::vector<CircleObstacle> *cobstacles,
                                 std::vector<SegmentObstacle> *sobstacles);
                 bool add_iy(RRTNode *n);
+                bool add_ixy(RRTNode *n);
                 bool goal_found(bool f);
 
                 // other