From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 28 Jan 2020 11:58:32 +0000 (+0100)
Subject: Add kd tree skeleton
X-Git-Tag: v0.4.0~16^2~4
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/ee3f29d83aace50b856c748e5e786fb2f2729495

Add kd tree skeleton
---

diff --git a/CMakeLists.txt b/CMakeLists.txt
index fe6d4b3..dd0d21d 100644
--- 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/rrtext7.cc
         src/rrtext6.cc
         src/rrtext5.cc
         src/rrtext4.cc
diff --git a/README.md b/README.md
index 2d419c3..9c5df8d 100644
--- a/README.md
+++ b/README.md
@@ -34,6 +34,7 @@ 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.
 - `rrtext6.cc`: Reeds and Shepp for both -- building and search costs,
 - `rrtext5.cc`: different cost for building (Reeds and Shepp) and searching
   (Euclidean distance),
@@ -44,6 +45,7 @@ There is a list of implemented extensions and what they include:
   (Matej's heuristics).
 
 [cute c2]: https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
+[K-d tree]: https://en.wikipedia.org/wiki/K-d_tree
 
 ## Compound extensions
 There is a list of classes with reference to extensions used:
diff --git a/api/rrtext.h b/api/rrtext.h
index ee95810..9d34ae2 100644
--- a/api/rrtext.h
+++ b/api/rrtext.h
@@ -13,6 +13,40 @@
 #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 2D K-d tree.
+
+\see https://en.wikipedia.org/wiki/K-d_tree
+*/
+class RRTExt7 : 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 store_node(RRTNode *n, KdNode *&r, int l);
+        public:
+                void init();
+                void deinit();
+                void store_node(RRTNode n);
+                RRTNode *nn(RRTNode &t);
+                std::vector<RRTNode *> nv(RRTNode &t);
+};
+
 /*! \brief Reeds and Shepp cost for building and search.
 */
 class RRTExt6 : public virtual RRTS {
diff --git a/src/rrtext7.cc b/src/rrtext7.cc
new file mode 100644
index 0000000..3481034
--- /dev/null
+++ b/src/rrtext7.cc
@@ -0,0 +1,50 @@
+#include "rrtext.h"
+
+RRTExt7::KdNode::KdNode(RRTNode *n)
+        : node_(n)
+        , left_(nullptr)
+        , right_(nullptr)
+{
+}
+
+void RRTExt7::store_node(RRTNode *n, KdNode *&r, int l)
+{
+        if (r == nullptr)
+                r = new KdNode(n);
+        else if (l % 2 == 0 && n->x() < r->node()->x())
+                store_node(n, r->left(), l + 1);
+        else if (l % 2 == 0)
+                store_node(n, r->right(), l + 1);
+        else if (l % 2 == 1 && n->y() < r->node()->y())
+                store_node(n, r->left(), l + 1);
+        else
+                store_node(n, r->right(), l + 1);
+}
+
+// API
+void RRTExt7::init()
+{
+}
+
+void RRTExt7::deinit()
+{
+}
+
+void RRTExt7::store_node(RRTNode n)
+{
+        RRTS::store_node(n);
+        RRTNode *sn = &this->nodes().back();
+        this->store_node(sn, this->kdroot_, 0);
+}
+
+RRTNode *RRTExt7::nn(RRTNode &t)
+{
+        RRTNode *nn = this->kdroot_->node();
+        return nn;
+}
+
+std::vector<RRTNode *> RRTExt7::nv(RRTNode &t)
+{
+        std::vector<RRTNode *> nv;
+        return nv;
+}