Copy skeleton from ext4

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4cac3768984d1b0391ba612756fe762b4a853704..afb5439e149f5715f5e35fdfcbcb090b2fa5c808 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -15,6 +15,7 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}/api)
 
 add_library(rrts SHARED
         src/rrts.cc
+        src/rrtext9.cc
         src/rrtext8.cc
         src/rrtext7.cc
         src/rrtext6.cc

diff --git a/README.md b/README.md
index 67e71490b4ecb07e83a97390803553b0af519245..4fbda8e37e3e03f1b7959221779c447927b75ee0 100644 (file)
--- a/README.md
+++ b/README.md
@@ -34,12 +34,13 @@ and upgrades to RRT, *extensions* are declared in `rrtext.h` and implemented in
 
 ## Implemented extensions
 There is a list of implemented extensions and what they include:
+- `rrtext9.cc`: store RRT nodes to 3D grid,
 - `rrtext8.cc`: 3D [K-d tree][] for node storage.
 - `rrtext7.cc`: 2D [K-d tree][] for node storage.
 - `rrtext6.cc`: Reeds and Shepp for both -- building and search costs,
 - `rrtext5.cc`: different cost for building (Reeds and Shepp) and searching
   (Euclidean distance),
-- `rrtext4.cc`: store RRT nodes to grid,
+- `rrtext4.cc`: store RRT nodes to 2D grid,
 - `rrtext3.cc`: Dijkstra algorithm for path optimization,
 - `rrtext2.cc`: [cute c2][] for collision detection,
 - `rrtext1.cc`: different cost for building (Reeds and Shepp) and searching

diff --git a/api/rrtext.h b/api/rrtext.h
index 128fe0c49334d7674b06ee454569d95f5439810a..ed246f9066ebacbd4ce87d7dc021c59663924741 100644 (file)
--- a/api/rrtext.h
+++ b/api/rrtext.h
@@ -13,6 +13,49 @@
 #define GRID_MAX_XI ((unsigned int) floor(GRID_WIDTH / GRID)) // min is 0
 #define GRID_MAX_YI ((unsigned int) floor(GRID_HEIGHT / GRID)) // min is 0
 
+/*! \brief Use grid data structure to store RRT nodes.
+
+This approach speeds up the search process for the nearest neighbor and
+the near vertices procedures.
+*/
+class RRTExt9 : public virtual RRTS {
+        private:
+                class Cell {
+                        private:
+                                bool changed_ = false;
+                                std::vector<RRTNode *> nodes_;
+                        public:
+                                void nn(RRTNode *t, RRTNode **nn, RRTS *p);
+                                void store_node(RRTNode *n);
+
+                                // getter, setter
+                                bool changed() const
+                                {
+                                        return this->changed_;
+                                }
+                                std::vector<RRTNode *> &nodes()
+                                {
+                                        return this->nodes_;
+                                }
+
+                                Cell();
+                };
+                Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left
+                unsigned int x_min_ = 0;
+                unsigned int x_max_ = 0;
+                unsigned int y_min_ = 0;
+                unsigned int y_max_ = 0;
+
+                unsigned int xi(RRTNode n);
+                unsigned int yi(RRTNode n);
+        public:
+                void init();
+                void deinit();
+                void store_node(RRTNode n);
+                RRTNode *nn(RRTNode &t);
+                std::vector<RRTNode *> nv(RRTNode &t);
+};
+
 /*! \brief Use k-d tree data structure to store RRT nodes.
 
 This approach speeds up the search process for the nearest neighbor and

diff --git a/src/rrtext9.cc b/src/rrtext9.cc
new file mode 100644 (file)
index 0000000..673ded3
--- /dev/null
+++ b/src/rrtext9.cc
@@ -0,0 +1,112 @@
+#include "rrtext.h"
+
+void RRTExt9::Cell::nn(RRTNode *t, RRTNode **nn, RRTS *p)
+{
+        double cost = p->cost_search(**nn, *t);
+        for (auto f: this->nodes()) {
+                if (p->cost_search(*f, *t) < cost) {
+                        *nn = f;
+                        cost = p->cost_search(*f, *t);
+                }
+        }
+}
+
+void RRTExt9::Cell::store_node(RRTNode *n)
+{
+        this->nodes_.push_back(n);
+        this->changed_ = true;
+}
+
+RRTExt9::Cell::Cell()
+{
+}
+
+unsigned int RRTExt9::xi(RRTNode n)
+{
+        if (n.x() >= this->x_max_)
+                return GRID_MAX_XI - 1;
+        if (n.x() <= this->x_min_)
+                return 0;
+        return (unsigned int) (floor(n.x() - this->x_min_) / GRID);
+}
+
+unsigned int RRTExt9::yi(RRTNode n)
+{
+        if (n.y() > this->y_max_)
+                return GRID_MAX_YI - 1;
+        if (n.y() <= this->y_min_)
+                return 0;
+        return (unsigned int) (floor(n.y() - this->y_min_) / GRID);
+}
+
+// API
+void RRTExt9::init()
+{
+        this->x_min_ = this->nodes().back().x() - GRID_WIDTH / 2;
+        this->x_max_ = this->nodes().back().x() + GRID_WIDTH / 2;
+        this->y_min_ = this->nodes().back().y() - GRID_HEIGHT / 2;
+        this->y_max_ = this->nodes().back().y() + GRID_HEIGHT / 2;
+}
+
+void RRTExt9::deinit()
+{
+        for (unsigned int i = 0; i < GRID_MAX_XI; i++)
+                for (unsigned int j = 0; j < GRID_MAX_YI; j++)
+                        this->grid_[i][j].nodes().clear();
+}
+
+void RRTExt9::store_node(RRTNode n)
+{
+        RRTS::store_node(n);
+        RRTNode *sn = &this->nodes().back();
+        this->grid_[this->xi(n)][this->yi(n)].store_node(sn);
+}
+
+RRTNode *RRTExt9::nn(RRTNode &t)
+{
+        RRTNode *nn = &this->nodes().front();
+        unsigned int txi = this->xi(t);
+        unsigned int tyi = this->yi(t);
+        unsigned int l = 0;
+        while (this->cost_search(*nn, t) > l * GRID) {
+                int xi_min = txi - l;
+                if (xi_min < 0)
+                        xi_min = 0;
+                int xi_max = txi + l;
+                if (xi_max > GRID_MAX_XI - 1)
+                        xi_max = GRID_MAX_XI - 1;
+                int yi_min = tyi - l;
+                if (yi_min < 0)
+                        yi_min = 0;
+                int yi_max = tyi + l;
+                if (yi_max > GRID_MAX_YI - 1)
+                        yi_max = GRID_MAX_YI - 1;
+                for (int xi = xi_min; xi <= xi_max; xi++) {
+                        this->grid_[xi][yi_max].nn(&t, &nn, this);
+                }
+                for (int xi = xi_min; xi <= xi_max; xi++) {
+                        this->grid_[xi][yi_min].nn(&t, &nn, this);
+                }
+                for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
+                        this->grid_[xi_min][yi].nn(&t, &nn, this);
+                }
+                for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
+                        this->grid_[xi_max][yi].nn(&t, &nn, this);
+                }
+                l++;
+        }
+        return nn;
+}
+
+std::vector<RRTNode *> RRTExt9::nv(RRTNode &t)
+{
+        std::vector<RRTNode *> nv;
+        unsigned int txi = this->xi(t);
+        unsigned int tyi = this->yi(t);
+        unsigned int l = 0;
+        double cost = std::min(GAMMA(this->nodes().size()), ETA);
+        for (auto f: this->grid_[txi][tyi].nodes())
+                if (this->cost_search(*f, t) < cost)
+                        nv.push_back(f);
+        return nv;
+}