Rewrite ext8

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1d241c27143573db9116f047b6977e638adfa8db..30f9759a3f474c693cac228696e6db917d212ee1 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -19,6 +19,7 @@ add_library(rrts STATIC
        src/rrts.cc
        src/rrtext14.cc
        src/rrtext10.cc
+       src/rrtext8.cc
        src/rrtext2.cc
        src/reeds_shepp.cpp
 )

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 711655fad2dc5652cc7c0f9f79106c7851e57bb5..299f6202c7583756029decbb51d590118e736b8d 100644 (file)
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -150,49 +150,32 @@ class RRTExt9 : public virtual RRTS {
 };
 
 /*! \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
-*/
+ *
+ * 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);
-               void nv(
-                       std::vector<RRTNode*>& n,
-                       RRTNode &t,
-                       KdNode *r,
-                       int l,
-                       double const& d
-               );
+private:
+       class KdNode {
        public:
-               void delete_kd_nodes()
-               {
-                   this->delete_kd_nodes(this->kdroot_);
-                   this->kdroot_ = nullptr;
-               }
-               void init();
-               void reset();
-               void deinit();
-               void store_node(RRTNode n);
-               RRTNode *nn(RRTNode &t);
-               std::vector<RRTNode *> nv(RRTNode &t);
+               RRTNode* node = nullptr;
+               KdNode* left = nullptr;
+               KdNode* right = nullptr;
+               KdNode(RRTNode* n);
+       };
+       KdNode* kdroot_ = nullptr;
+       std::vector<KdNode> kdnodes_;
+       void store(RRTNode* n, KdNode*& ref, unsigned int lvl);
+       void find_nn(RRTNode const& t, KdNode const* const r, unsigned int lvl);
+       void find_nv(RRTNode const& t, KdNode const* const r, unsigned int lvl);
+public:
+       RRTExt8();
+       void reset();
+       void store(RRTNode n);
+       void find_nn(RRTNode const& t);
+       void find_nv(RRTNode const& t);
 };
 
 /*! \brief Use k-d tree data structure to store RRT nodes.

diff --git a/src/rrtext8.cc b/src/rrtext8.cc
index 13032a25b584d3658b27ae84abcda396552043df..95bc6709253ad222f122140e303dc06ba960f8e6 100644 (file)
--- a/src/rrtext8.cc
+++ b/src/rrtext8.cc
@@ -1,149 +1,154 @@
-#include "rrtext.h"
+#include "rrtext.hh"
 
-RRTExt8::KdNode::KdNode(RRTNode *n)
-       : node_(n)
-       , left_(nullptr)
-       , right_(nullptr)
-{
-}
+namespace rrts {
 
-void RRTExt8::delete_kd_nodes(KdNode *n)
+RRTExt8::KdNode::KdNode(RRTNode* n) : node(n)
 {
-       if (n == nullptr)
-               return;
-       this->delete_kd_nodes(n->left());
-       this->delete_kd_nodes(n->right());
-       delete n;
 }
 
-void RRTExt8::store_node(RRTNode *n, KdNode *&r, int l)
+void
+RRTExt8::store(RRTNode* n, KdNode*& ref, unsigned int lvl)
 {
-       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 == 2 && n->h() < r->node()->h())
-               store_node(n, r->left(), l + 1);
-       else
-               store_node(n, r->right(), l + 1);
+       if (ref == nullptr) {
+               this->kdnodes_.push_back(KdNode(n));
+               ref = &this->kdnodes_.back();
+       } else if (lvl % 3 == 0 && n->x() < ref->node->x()) {
+               this->store(n, ref->left, lvl + 1);
+       } else if (lvl % 3 == 0) {
+               this->store(n, ref->right, lvl + 1);
+       } else if (lvl % 3 == 1 && n->y() < ref->node->y()) {
+               this->store(n, ref->left, lvl + 1);
+       } else if (lvl % 3 == 1) {
+               this->store(n, ref->right, lvl + 1);
+       } else if (lvl % 3 == 2 && n->h() < ref->node->h()) {
+               this->store(n, ref->left, lvl + 1);
+       } else {
+               this->store(n, ref->right, lvl + 1);
+       }
 }
 
-void RRTExt8::nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d)
+void
+RRTExt8::find_nn(RRTNode const& t, KdNode const* const ref, unsigned int lvl)
 {
-       if (r == nullptr)
+       if (ref == 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);
+       if (this->cost_search(*ref->node, t) < this->cost_
+                       || this->nn_ == nullptr) {
+               this->nn_ = ref->node;
+               this->cost_ = this->cost_search(*ref->node, t);
+       }
+       if (lvl % 3 == 0 && t.x() < ref->node->x()) {
+               this->find_nn(t, ref->left, lvl + 1);
+               if (ref->node->x() - t.x() < this->cost_) {
+                       this->find_nn(t, ref->right, lvl + 1);
+               }
+       } else if (lvl % 3 == 0) {
+               this->find_nn(t, ref->right, lvl + 1);
+               if (t.x() - ref->node->x() < this->cost_) {
+                       this->find_nn(t, ref->left, lvl + 1);
+               }
+       } else if (lvl % 3 == 1 && t.y() < ref->node->y()) {
+               this->find_nn(t, ref->left, lvl + 1);
+               if (ref->node->y() - t.y() < this->cost_) {
+                       this->find_nn(t, ref->right, lvl + 1);
+               }
+       } else if (lvl % 3 == 1) {
+               this->find_nn(t, ref->right, lvl + 1);
+               if (t.y() - ref->node->y() < this->cost_) {
+                       this->find_nn(t, ref->left, lvl + 1);
+               }
+       } else if (lvl % 3 == 2 && t.h() < ref->node->h()) {
+               this->find_nn(t, ref->left, lvl + 1);
+               if (ref->node->h() - t.h() < this->cost_) {
+                       this->find_nn(t, ref->right, lvl + 1);
+               }
        } else {
-               nn(n, t, r->right(), l + 1, d);
-               if (t.h() - r->node()->h() < d)
-                       nn(n, t, r->left(), l + 1, d);
+               this->find_nn(t, ref->right, lvl + 1);
+               if (t.h() - ref->node->h() < this->cost_) {
+                       this->find_nn(t, ref->left, lvl + 1);
+               }
        }
 }
 
-void RRTExt8::nv(
-       std::vector<RRTNode*>& n,
-       RRTNode &t,
-       KdNode *r,
-       int l,
-       double const& d
-)
+void
+RRTExt8::find_nv(RRTNode const& t, KdNode const* const ref, unsigned int lvl)
 {
-       if (r == nullptr)
+       if (ref == nullptr) {
                return;
-       if (this->cost_search(*r->node(), t) < d) {
-               n.push_back(r->node());
        }
-       if (l % 3 == 0 && t.x() < r->node()->x()) {
-               this->nv(n, t, r->left(), l + 1, d);
-               if (r->node()->x() - t.x() < d)
-                       this->nv(n, t, r->right(), l + 1, d);
-       } else if (l % 3 == 0) {
-               this->nv(n, t, r->right(), l + 1, d);
-               if (t.x() - r->node()->x() < d)
-                       this->nv(n, t, r->left(), l + 1, d);
-       } else if (l % 3 == 1 && t.y() < r->node()->y()) {
-               this->nv(n, t, r->left(), l + 1, d);
-               if (r->node()->y() - t.y() < d)
-                       this->nv(n, t, r->right(), l + 1, d);
-       } else if (l % 3 == 1) {
-               this->nv(n, t, r->right(), l + 1, d);
-               if (t.y() - r->node()->y() < d)
-                       this->nv(n, t, r->left(), l + 1, d);
-       } else if (l % 3 == 2 && t.h() < r->node()->h()) {
-               this->nv(n, t, r->left(), l + 1, d);
-               if (r->node()->h() - t.h() < d)
-                       this->nv(n, t, r->right(), l + 1, d);
+       if (this->cost_search(*ref->node, t) < this->cost_) {
+               this->nv_.push_back(ref->node);
+       }
+       if (lvl % 3 == 0 && t.x() < ref->node->x()) {
+               this->find_nv(t, ref->left, lvl + 1);
+               if (ref->node->x() - t.x() < this->cost_) {
+                       this->find_nv(t, ref->right, lvl + 1);
+               }
+       } else if (lvl % 3 == 0) {
+               this->find_nv(t, ref->right, lvl + 1);
+               if (t.x() - ref->node->x() < this->cost_) {
+                       this->find_nv(t, ref->left, lvl + 1);
+               }
+       } else if (lvl % 3 == 1 && t.y() < ref->node->y()) {
+               this->find_nv(t, ref->left, lvl + 1);
+               if (ref->node->y() - t.y() < this->cost_) {
+                       this->find_nv(t, ref->right, lvl + 1);
+               }
+       } else if (lvl % 3 == 1) {
+               this->find_nv(t, ref->right, lvl + 1);
+               if (t.y() - ref->node->y() < this->cost_) {
+                       this->find_nv(t, ref->left, lvl + 1);
+               }
+       } else if (lvl % 3 == 2 && t.h() < ref->node->h()) {
+               this->find_nv(t, ref->left, lvl + 1);
+               if (ref->node->h() - t.h() < this->cost_) {
+                       this->find_nv(t, ref->right, lvl + 1);
+               }
        } else {
-               this->nv(n, t, r->right(), l + 1, d);
-               if (t.h() - r->node()->h() < d)
-                       this->nv(n, t, r->left(), l + 1, d);
+               this->find_nv(t, ref->right, lvl + 1);
+               if (t.h() - ref->node->h() < this->cost_) {
+                       this->find_nv(t, ref->left, lvl + 1);
+               }
        }
 }
 
-// API
-void RRTExt8::init()
+RRTExt8::RRTExt8() : RRTS()
 {
+       this->kdnodes_.reserve(this->nodes_.capacity());
+       this->store(&this->nodes_.front(), this->kdroot_, 0);
 }
 
-void RRTExt8::reset()
+void
+RRTExt8::reset()
 {
        RRTS::reset();
-       this->delete_kd_nodes();
+       this->kdnodes_.clear();
+       this->kdroot_ = nullptr;
+       this->store(&this->nodes_.front(), this->kdroot_, 0);
 }
 
-void RRTExt8::deinit()
+void
+RRTExt8::store(RRTNode n)
 {
-       this->delete_kd_nodes(this->kdroot_);
+       RRTS::store(n);
+       this->store(&this->nodes_.back(), this->kdroot_, 0);
 }
 
-void RRTExt8::store_node(RRTNode n)
+void
+RRTExt8::find_nn(RRTNode const& t)
 {
-       RRTS::store_node(n);
-       RRTNode *sn = &this->nodes().back();
-       this->store_node(sn, this->kdroot_, 0);
+       this->nn_ = &this->nodes_.front();
+       this->cost_ = this->cost_search(*this->nn_, t);
+       this->find_nn(t, this->kdroot_, 0);
 }
 
-RRTNode *RRTExt8::nn(RRTNode &t)
+void
+RRTExt8::find_nv(RRTNode const& t)
 {
-       RRTNode *n = &this->nodes().front();
-       double d = 9999;
-       this->nn(n, t, this->kdroot_, 0, d);
-       return n;
+       this->nv_.clear();
+       this->cost_ = this->min_gamma_eta();
+       this->find_nv(t, this->kdroot_, 0);
 }
 
-std::vector<RRTNode *> RRTExt8::nv(RRTNode &t)
-{
-       double cost = std::min(GAMMA(this->nodes().size()), 0.5);
-       std::vector<RRTNode *> n;
-       this->nv(n, t, this->kdroot_, 0, cost);
-       return n;
-}
+} // namespace rrts