2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
11 RRTExt8::KdNode::KdNode(RRTNode* n) : node(n)
16 RRTExt8::store(RRTNode* n, KdNode*& ref, unsigned int lvl)
19 this->kdnodes_.push_back(KdNode(n));
20 ref = &this->kdnodes_.back();
21 } else if (lvl % 3 == 0 && n->x() < ref->node->x()) {
22 this->store(n, ref->left, lvl + 1);
23 } else if (lvl % 3 == 0) {
24 this->store(n, ref->right, lvl + 1);
25 } else if (lvl % 3 == 1 && n->y() < ref->node->y()) {
26 this->store(n, ref->left, lvl + 1);
27 } else if (lvl % 3 == 1) {
28 this->store(n, ref->right, lvl + 1);
29 } else if (lvl % 3 == 2 && n->h() < ref->node->h()) {
30 this->store(n, ref->left, lvl + 1);
32 this->store(n, ref->right, lvl + 1);
37 RRTExt8::find_nn(RRTNode const& t, KdNode const* const ref, unsigned int lvl)
42 if (this->cost_search(*ref->node, t) < this->cost_
43 || this->nn_ == nullptr) {
44 this->nn_ = ref->node;
45 this->cost_ = this->cost_search(*ref->node, t);
47 if (lvl % 3 == 0 && t.x() < ref->node->x()) {
48 this->find_nn(t, ref->left, lvl + 1);
49 if (ref->node->x() - t.x() < this->cost_) {
50 this->find_nn(t, ref->right, lvl + 1);
52 } else if (lvl % 3 == 0) {
53 this->find_nn(t, ref->right, lvl + 1);
54 if (t.x() - ref->node->x() < this->cost_) {
55 this->find_nn(t, ref->left, lvl + 1);
57 } else if (lvl % 3 == 1 && t.y() < ref->node->y()) {
58 this->find_nn(t, ref->left, lvl + 1);
59 if (ref->node->y() - t.y() < this->cost_) {
60 this->find_nn(t, ref->right, lvl + 1);
62 } else if (lvl % 3 == 1) {
63 this->find_nn(t, ref->right, lvl + 1);
64 if (t.y() - ref->node->y() < this->cost_) {
65 this->find_nn(t, ref->left, lvl + 1);
67 } else if (lvl % 3 == 2 && t.h() < ref->node->h()) {
68 this->find_nn(t, ref->left, lvl + 1);
69 if (ref->node->h() - t.h() < this->cost_) {
70 this->find_nn(t, ref->right, lvl + 1);
73 this->find_nn(t, ref->right, lvl + 1);
74 if (t.h() - ref->node->h() < this->cost_) {
75 this->find_nn(t, ref->left, lvl + 1);
81 RRTExt8::find_nv(RRTNode const& t, KdNode const* const ref, unsigned int lvl)
86 if (this->cost_search(*ref->node, t) < this->cost_) {
87 this->nv_.push_back(ref->node);
89 if (lvl % 3 == 0 && t.x() < ref->node->x()) {
90 this->find_nv(t, ref->left, lvl + 1);
91 if (ref->node->x() - t.x() < this->cost_) {
92 this->find_nv(t, ref->right, lvl + 1);
94 } else if (lvl % 3 == 0) {
95 this->find_nv(t, ref->right, lvl + 1);
96 if (t.x() - ref->node->x() < this->cost_) {
97 this->find_nv(t, ref->left, lvl + 1);
99 } else if (lvl % 3 == 1 && t.y() < ref->node->y()) {
100 this->find_nv(t, ref->left, lvl + 1);
101 if (ref->node->y() - t.y() < this->cost_) {
102 this->find_nv(t, ref->right, lvl + 1);
104 } else if (lvl % 3 == 1) {
105 this->find_nv(t, ref->right, lvl + 1);
106 if (t.y() - ref->node->y() < this->cost_) {
107 this->find_nv(t, ref->left, lvl + 1);
109 } else if (lvl % 3 == 2 && t.h() < ref->node->h()) {
110 this->find_nv(t, ref->left, lvl + 1);
111 if (ref->node->h() - t.h() < this->cost_) {
112 this->find_nv(t, ref->right, lvl + 1);
115 this->find_nv(t, ref->right, lvl + 1);
116 if (t.h() - ref->node->h() < this->cost_) {
117 this->find_nv(t, ref->left, lvl + 1);
122 RRTExt8::RRTExt8() : RRTS()
124 this->kdnodes_.reserve(this->nodes_.capacity());
125 this->store(&this->nodes_.front(), this->kdroot_, 0);
132 this->kdnodes_.clear();
133 this->kdroot_ = nullptr;
134 this->store(&this->nodes_.front(), this->kdroot_, 0);
138 RRTExt8::store(RRTNode n)
141 this->store(&this->nodes_.back(), this->kdroot_, 0);
145 RRTExt8::find_nn(RRTNode const& t)
147 this->nn_ = &this->nodes_.front();
148 this->cost_ = this->cost_search(*this->nn_, t);
149 this->find_nn(t, this->kdroot_, 0);
153 RRTExt8::find_nv(RRTNode const& t)
156 this->cost_ = this->min_gamma_eta();
157 this->find_nv(t, this->kdroot_, 0);