2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
9 RRTExt7::KdNode::KdNode(RRTNode *n)
16 void RRTExt7::delete_kd_nodes(KdNode *n)
20 if (n->left() != nullptr)
21 delete_kd_nodes(n->left());
22 if (n->right() != nullptr)
23 delete_kd_nodes(n->right());
27 void RRTExt7::store_node(RRTNode *n, KdNode *&r, int l)
31 else if (l % 2 == 0 && n->x() < r->node()->x())
32 store_node(n, r->left(), l + 1);
34 store_node(n, r->right(), l + 1);
35 else if (l % 2 == 1 && n->y() < r->node()->y())
36 store_node(n, r->left(), l + 1);
38 store_node(n, r->right(), l + 1);
41 void RRTExt7::nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d)
45 if (this->cost_search(*r->node(), t) < d) {
47 d = this->cost_search(*r->node(), t);
49 if (l % 2 == 0 && t.x() < r->node()->x()) {
50 nn(n, t, r->left(), l + 1, d);
51 if (r->node()->x() - t.x() < d)
52 nn(n, t, r->right(), l + 1, d);
53 } else if (l % 2 == 0) {
54 nn(n, t, r->right(), l + 1, d);
55 if (t.x() - r->node()->x() < d)
56 nn(n, t, r->left(), l + 1, d);
57 } else if (l % 2 == 1 && t.y() < r->node()->y()) {
58 nn(n, t, r->left(), l + 1, d);
59 if (r->node()->y() - t.y() < d)
60 nn(n, t, r->right(), l + 1, d);
62 nn(n, t, r->right(), l + 1, d);
63 if (t.y() - r->node()->y() < d)
64 nn(n, t, r->left(), l + 1, d);
73 void RRTExt7::deinit()
75 this->delete_kd_nodes(this->kdroot_);
78 void RRTExt7::store_node(RRTNode n)
81 RRTNode *sn = &this->nodes().back();
82 this->store_node(sn, this->kdroot_, 0);
85 RRTNode *RRTExt7::nn(RRTNode &t)
87 RRTNode *n = &this->nodes().front();
89 this->nn(n, t, this->kdroot_, 0, d);
93 std::vector<RRTNode *> RRTExt7::nv(RRTNode &t)
95 std::vector<RRTNode *> nv;