rrts/src/rrtext8.cc

   1 /*
   2  * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 #include "rrtext.hh"
   8
   9 namespace rrts {
  10
  11 RRTExt8::KdNode::KdNode(RRTNode* n) : node(n)
  12 {
  13 }
  14
  15 void
  16 RRTExt8::store(RRTNode* n, KdNode*& ref, unsigned int lvl)
  17 {
  18         if (ref == nullptr) {
  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);
  31         } else {
  32                 this->store(n, ref->right, lvl + 1);
  33         }
  34 }
  35
  36 void
  37 RRTExt8::find_nn(RRTNode const& t, KdNode const* const ref, unsigned int lvl)
  38 {
  39         if (ref == nullptr) {
  40                 return;
  41         }
  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);
  46         }
  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);
  51                 }
  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);
  56                 }
  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);
  61                 }
  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);
  66                 }
  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);
  71                 }
  72         } else {
  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);
  76                 }
  77         }
  78 }
  79
  80 void
  81 RRTExt8::find_nv(RRTNode const& t, KdNode const* const ref, unsigned int lvl)
  82 {
  83         if (ref == nullptr) {
  84                 return;
  85         }
  86         if (this->cost_search(*ref->node, t) < this->_cost) {
  87                 this->_nv.push_back(ref->node);
  88         }
  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);
  93                 }
  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);
  98                 }
  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);
 103                 }
 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);
 108                 }
 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);
 113                 }
 114         } else {
 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);
 118                 }
 119         }
 120 }
 121
 122 RRTExt8::RRTExt8() : RRTS()
 123 {
 124         this->_kdnodes.reserve(this->_nodes.capacity());
 125         this->store(&this->_nodes.front(), this->kdroot_, 0);
 126 }
 127
 128 void
 129 RRTExt8::reset()
 130 {
 131         RRTS::reset();
 132         this->_kdnodes.clear();
 133         this->kdroot_ = nullptr;
 134         this->store(&this->_nodes.front(), this->kdroot_, 0);
 135 }
 136
 137 void
 138 RRTExt8::store(RRTNode n)
 139 {
 140         RRTS::store(n);
 141         this->store(&this->_nodes.back(), this->kdroot_, 0);
 142 }
 143
 144 void
 145 RRTExt8::find_nn(RRTNode const& t)
 146 {
 147         this->_nn = &this->_nodes.front();
 148         this->_cost = this->cost_search(*this->_nn, t);
 149         this->find_nn(t, this->kdroot_, 0);
 150 }
 151
 152 void
 153 RRTExt8::find_nv(RRTNode const& t)
 154 {
 155         this->_nv.clear();
 156         this->_cost = this->min_gamma_eta();
 157         this->find_nv(t, this->kdroot_, 0);
 158 }
 159
 160 } // namespace rrts