Merge branch 'feature/3d-k-d-tree'

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 6a2fa085cf8bddb8aeed7839930d264661a29b22..31d0463d90afbb3f97cbf2e83dd4bb46aecbddab 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -27,7 +27,8 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Deinit method -- get ready for planner reset.
 - JSON input.
 - JSON output.
-- K-d tree extension.
+- 2D K-d tree extension.
+- 3D K-d tree extension.
 
 ### Changed
 - How the algorithm is stopped - possible to choose from more options.

diff --git a/CMakeLists.txt b/CMakeLists.txt
index dd0d21dc72b52405252ac53024efc4a8e91dd3ad..4cac3768984d1b0391ba612756fe762b4a853704 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/rrtext8.cc
         src/rrtext7.cc
         src/rrtext6.cc
         src/rrtext5.cc

diff --git a/README.md b/README.md
index 7d84f1e6e1a94d7f0647beac4bcef48fe15cbd32..67e71490b4ecb07e83a97390803553b0af519245 100644 (file)
--- a/README.md
+++ b/README.md
@@ -34,7 +34,8 @@ 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:
-- `rrtext7.cc`: [K-d tree][] for node storage.
+- `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),
@@ -61,6 +62,7 @@ There is a list of classes with reference to extensions used:
 - `RRTCE10`: 2, 3, 7, 6.
 - `RRTCE11`: 2, 3, 7, 5.
 - `RRTCE12`: 2, 3, 7, 1.
+- `RRTCE13`: 2, 3, 8, 6.
 
 # Contribute
 Use [OneFlow][3] branching model and keep the [changelog][4].

diff --git a/api/rrtce.h b/api/rrtce.h
index 5260d2d5243809b90d1465d3737ad83fcafbbcf4..6139cbd3aa434fb89b0f5d303d10bff6dde81e50 100644 (file)
--- a/api/rrtce.h
+++ b/api/rrtce.h
@@ -161,5 +161,23 @@ class RRTCE12
                         RRTExt7::deinit();
                 }
 };
+class RRTCE13
+        : public RRTExt2
+        , public RRTExt3
+        , public RRTExt8
+        , public RRTExt6
+{
+        public:
+                void init()
+                {
+                        RRTExt2::init();
+                        RRTExt8::init();
+                }
+                void deinit()
+                {
+                        RRTExt2::deinit();
+                        RRTExt8::deinit();
+                }
+};
 
 #endif /* RRTCE_H */

diff --git a/api/rrtext.h b/api/rrtext.h
index 2081c56eb99c25cff88a2e86fcdca36020c5f7f7..128fe0c49334d7674b06ee454569d95f5439810a 100644 (file)
--- a/api/rrtext.h
+++ b/api/rrtext.h
@@ -13,8 +13,38 @@
 #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
 
-// ext7
-#define MAX_DIM 2
+/*! \brief Use k-d tree data structure to store RRT nodes.
+
+This approach speeds up the search process for the nearest neighbor and
+the near vertices procedures. This extension implements 3D K-d tree.
+
+\see https://en.wikipedia.org/wiki/K-d_tree
+*/
+class RRTExt8 : public virtual RRTS {
+        private:
+                class KdNode {
+                        private:
+                                RRTNode *node_ = nullptr;
+                                KdNode *left_ = nullptr;
+                                KdNode *right_ = nullptr;
+                        public:
+                                // getter, setter
+                                RRTNode *node() const { return this->node_; }
+                                KdNode *&left() { return this->left_; }
+                                KdNode *&right() { return this->right_; }
+                                KdNode(RRTNode *n);
+                };
+                KdNode *kdroot_ = nullptr;
+                void delete_kd_nodes(KdNode *n);
+                void store_node(RRTNode *n, KdNode *&r, int l);
+                void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d);
+        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.
 

diff --git a/src/rrtext8.cc b/src/rrtext8.cc
new file mode 100644 (file)
index 0000000..861ea87
--- /dev/null
+++ b/src/rrtext8.cc
@@ -0,0 +1,103 @@
+#include "rrtext.h"
+
+RRTExt8::KdNode::KdNode(RRTNode *n)
+        : node_(n)
+        , left_(nullptr)
+        , right_(nullptr)
+{
+}
+
+void RRTExt8::delete_kd_nodes(KdNode *n)
+{
+        if (!n)
+                return;
+        if (n->left() != nullptr)
+                delete_kd_nodes(n->left());
+        if (n->right() != nullptr)
+                delete_kd_nodes(n->right());
+        delete n;
+}
+
+void RRTExt8::store_node(RRTNode *n, KdNode *&r, int l)
+{
+        if (r == nullptr)
+                r = new KdNode(n);
+        else if (l % 3 == 0 && n->x() < r->node()->x())
+                store_node(n, r->left(), l + 1);
+        else if (l % 3 == 0)
+                store_node(n, r->right(), l + 1);
+        else if (l % 3 == 1 && n->y() < r->node()->y())
+                store_node(n, r->left(), l + 1);
+        else if (l % 3 == 1)
+                store_node(n, r->right(), l + 1);
+        else if (l % 3 == 0 && n->h() < r->node()->h())
+                store_node(n, r->left(), l + 1);
+        else
+                store_node(n, r->right(), l + 1);
+}
+
+void RRTExt8::nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d)
+{
+        if (r == nullptr)
+                return;
+        if (this->cost_search(*r->node(), t) < d) {
+                n = r->node();
+                d = this->cost_search(*r->node(), t);
+        }
+        if (l % 3 == 0 && t.x() < r->node()->x()) {
+                nn(n, t, r->left(), l + 1, d);
+                if (r->node()->x() - t.x() < d)
+                        nn(n, t, r->right(), l + 1, d);
+        } else if (l % 3 == 0) {
+                nn(n, t, r->right(), l + 1, d);
+                if (t.x() - r->node()->x() < d)
+                        nn(n, t, r->left(), l + 1, d);
+        } else if (l % 3 == 1 && t.y() < r->node()->y()) {
+                nn(n, t, r->left(), l + 1, d);
+                if (r->node()->y() - t.y() < d)
+                        nn(n, t, r->right(), l + 1, d);
+        } else if (l % 3 == 1) {
+                nn(n, t, r->right(), l + 1, d);
+                if (t.y() - r->node()->y() < d)
+                        nn(n, t, r->left(), l + 1, d);
+        } else if (l % 3 == 2 && t.h() < r->node()->h()) {
+                nn(n, t, r->left(), l + 1, d);
+                if (r->node()->h() - t.h() < d)
+                        nn(n, t, r->right(), l + 1, d);
+        } else {
+                nn(n, t, r->right(), l + 1, d);
+                if (t.h() - r->node()->h() < d)
+                        nn(n, t, r->left(), l + 1, d);
+        }
+}
+
+// API
+void RRTExt8::init()
+{
+}
+
+void RRTExt8::deinit()
+{
+        this->delete_kd_nodes(this->kdroot_);
+}
+
+void RRTExt8::store_node(RRTNode n)
+{
+        RRTS::store_node(n);
+        RRTNode *sn = &this->nodes().back();
+        this->store_node(sn, this->kdroot_, 0);
+}
+
+RRTNode *RRTExt8::nn(RRTNode &t)
+{
+        RRTNode *n = &this->nodes().front();
+        double d = 9999;
+        this->nn(n, t, this->kdroot_, 0, d);
+        return n;
+}
+
+std::vector<RRTNode *> RRTExt8::nv(RRTNode &t)
+{
+        std::vector<RRTNode *> nv;
+        return nv;
+}