From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Wed, 16 Jan 2019 15:15:54 +0000 (+0100)
Subject: Add structure for indexing by x, y axes to RRTBase
X-Git-Tag: v0.5.0~3^2~7
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/fbd7aec8eef49349b5ad5469a65a84e34ffd28eb

Add structure for indexing by x, y axes to RRTBase
---

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 1038d97..59ef374 100644
--- 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 fc3362f..2799d84 100644
--- 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