From d21f5f36c659cf71b100e442017253157a27e0c7 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Thu, 15 Jul 2021 18:34:42 +0200 Subject: [PATCH] Rewrite ext8 --- CMakeLists.txt | 1 + incl/rrtext.hh | 65 ++++++-------- src/rrtext8.cc | 227 +++++++++++++++++++++++++------------------------ 3 files changed, 141 insertions(+), 152 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1d241c2..30f9759 100644 --- 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 711655f..299f620 100644 --- 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& 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 nv(RRTNode &t); + RRTNode* node = nullptr; + KdNode* left = nullptr; + KdNode* right = nullptr; + KdNode(RRTNode* n); + }; + KdNode* kdroot_ = nullptr; + std::vector 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 13032a2..95bc670 100644 --- 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& 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 RRTExt8::nv(RRTNode &t) -{ - double cost = std::min(GAMMA(this->nodes().size()), 0.5); - std::vector n; - this->nv(n, t, this->kdroot_, 0, cost); - return n; -} +} // namespace rrts -- 2.39.2