5 RRTExt8::KdNode::KdNode(RRTNode* n) : node(n)
10 RRTExt8::store(RRTNode* n, KdNode*& ref, unsigned int lvl)
13 this->kdnodes_.push_back(KdNode(n));
14 ref = &this->kdnodes_.back();
15 } else if (lvl % 3 == 0 && n->x() < ref->node->x()) {
16 this->store(n, ref->left, lvl + 1);
17 } else if (lvl % 3 == 0) {
18 this->store(n, ref->right, lvl + 1);
19 } else if (lvl % 3 == 1 && n->y() < ref->node->y()) {
20 this->store(n, ref->left, lvl + 1);
21 } else if (lvl % 3 == 1) {
22 this->store(n, ref->right, lvl + 1);
23 } else if (lvl % 3 == 2 && n->h() < ref->node->h()) {
24 this->store(n, ref->left, lvl + 1);
26 this->store(n, ref->right, lvl + 1);
31 RRTExt8::find_nn(RRTNode const& t, KdNode const* const ref, unsigned int lvl)
36 if (this->cost_search(*ref->node, t) < this->cost_
37 || this->nn_ == nullptr) {
38 this->nn_ = ref->node;
39 this->cost_ = this->cost_search(*ref->node, t);
41 if (lvl % 3 == 0 && t.x() < ref->node->x()) {
42 this->find_nn(t, ref->left, lvl + 1);
43 if (ref->node->x() - t.x() < this->cost_) {
44 this->find_nn(t, ref->right, lvl + 1);
46 } else if (lvl % 3 == 0) {
47 this->find_nn(t, ref->right, lvl + 1);
48 if (t.x() - ref->node->x() < this->cost_) {
49 this->find_nn(t, ref->left, lvl + 1);
51 } else if (lvl % 3 == 1 && t.y() < ref->node->y()) {
52 this->find_nn(t, ref->left, lvl + 1);
53 if (ref->node->y() - t.y() < this->cost_) {
54 this->find_nn(t, ref->right, lvl + 1);
56 } else if (lvl % 3 == 1) {
57 this->find_nn(t, ref->right, lvl + 1);
58 if (t.y() - ref->node->y() < this->cost_) {
59 this->find_nn(t, ref->left, lvl + 1);
61 } else if (lvl % 3 == 2 && t.h() < ref->node->h()) {
62 this->find_nn(t, ref->left, lvl + 1);
63 if (ref->node->h() - t.h() < this->cost_) {
64 this->find_nn(t, ref->right, lvl + 1);
67 this->find_nn(t, ref->right, lvl + 1);
68 if (t.h() - ref->node->h() < this->cost_) {
69 this->find_nn(t, ref->left, lvl + 1);
75 RRTExt8::find_nv(RRTNode const& t, KdNode const* const ref, unsigned int lvl)
80 if (this->cost_search(*ref->node, t) < this->cost_) {
81 this->nv_.push_back(ref->node);
83 if (lvl % 3 == 0 && t.x() < ref->node->x()) {
84 this->find_nv(t, ref->left, lvl + 1);
85 if (ref->node->x() - t.x() < this->cost_) {
86 this->find_nv(t, ref->right, lvl + 1);
88 } else if (lvl % 3 == 0) {
89 this->find_nv(t, ref->right, lvl + 1);
90 if (t.x() - ref->node->x() < this->cost_) {
91 this->find_nv(t, ref->left, lvl + 1);
93 } else if (lvl % 3 == 1 && t.y() < ref->node->y()) {
94 this->find_nv(t, ref->left, lvl + 1);
95 if (ref->node->y() - t.y() < this->cost_) {
96 this->find_nv(t, ref->right, lvl + 1);
98 } else if (lvl % 3 == 1) {
99 this->find_nv(t, ref->right, lvl + 1);
100 if (t.y() - ref->node->y() < this->cost_) {
101 this->find_nv(t, ref->left, lvl + 1);
103 } else if (lvl % 3 == 2 && t.h() < ref->node->h()) {
104 this->find_nv(t, ref->left, lvl + 1);
105 if (ref->node->h() - t.h() < this->cost_) {
106 this->find_nv(t, ref->right, lvl + 1);
109 this->find_nv(t, ref->right, lvl + 1);
110 if (t.h() - ref->node->h() < this->cost_) {
111 this->find_nv(t, ref->left, lvl + 1);
116 RRTExt8::RRTExt8() : RRTS()
118 this->kdnodes_.reserve(this->nodes_.capacity());
119 this->store(&this->nodes_.front(), this->kdroot_, 0);
126 this->kdnodes_.clear();
127 this->kdroot_ = nullptr;
128 this->store(&this->nodes_.front(), this->kdroot_, 0);
132 RRTExt8::store(RRTNode n)
135 this->store(&this->nodes_.back(), this->kdroot_, 0);
139 RRTExt8::find_nn(RRTNode const& t)
141 this->nn_ = &this->nodes_.front();
142 this->cost_ = this->cost_search(*this->nn_, t);
143 this->find_nn(t, this->kdroot_, 0);
147 RRTExt8::find_nv(RRTNode const& t)
150 this->cost_ = this->min_gamma_eta();
151 this->find_nv(t, this->kdroot_, 0);