-#include "rrtext.h"
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
-RRTExt8::KdNode::KdNode(RRTNode *n)
- : node_(n)
- , left_(nullptr)
- , right_(nullptr)
-{
-}
+#include "rrtext.hh"
-void RRTExt8::delete_kd_nodes(KdNode *n)
+namespace rrts {
+
+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)
- 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);
- }
+ if (ref == nullptr) {
+ return;
+ }
+ 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 {
+ 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)
- 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);
- } 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);
- }
+ if (ref == nullptr) {
+ return;
+ }
+ 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->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::deinit()
+void
+RRTExt8::reset()
{
- this->delete_kd_nodes(this->kdroot_);
+ RRTS::reset();
+ this->kdnodes_.clear();
+ this->kdroot_ = nullptr;
+ this->store(&this->nodes_.front(), this->kdroot_, 0);
}
-void RRTExt8::store_node(RRTNode n)
+void
+RRTExt8::store(RRTNode n)
{
- RRTS::store_node(n);
- RRTNode *sn = &this->nodes().back();
- this->store_node(sn, this->kdroot_, 0);
+ RRTS::store(n);
+ this->store(&this->nodes_.back(), this->kdroot_, 0);
}
-RRTNode *RRTExt8::nn(RRTNode &t)
+void
+RRTExt8::find_nn(RRTNode const& t)
{
- RRTNode *n = &this->nodes().front();
- double d = 9999;
- this->nn(n, t, this->kdroot_, 0, d);
- return n;
+ this->nn_ = &this->nodes_.front();
+ this->cost_ = this->cost_search(*this->nn_, t);
+ this->find_nn(t, this->kdroot_, 0);
}
-std::vector<RRTNode *> RRTExt8::nv(RRTNode &t)
+void
+RRTExt8::find_nv(RRTNode const& t)
{
- double cost = std::min(GAMMA(this->nodes().size()), 0.2);
- std::vector<RRTNode *> n;
- this->nv(n, t, this->kdroot_, 0, cost);
- return n;
+ this->nv_.clear();
+ this->cost_ = this->min_gamma_eta();
+ this->find_nv(t, this->kdroot_, 0);
}
+
+} // namespace rrts
projects / hubacji1 / rrts.git / blobdiff