Copy kd tree from 2D kd tree

[hubacji1/rrts.git] / api / rrtext.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.