src/rrtext8.cc

   1 #include "rrtext.hh"
   2
   3 namespace rrts {
   4
   5 RRTExt8::KdNode::KdNode(RRTNode* n) : node(n)
   6 {
   7 }
   8
   9 void
  10 RRTExt8::store(RRTNode* n, KdNode*& ref, unsigned int lvl)
  11 {
  12         if (ref == nullptr) {
  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);
  25         } else {
  26                 this->store(n, ref->right, lvl + 1);
  27         }
  28 }
  29
  30 void
  31 RRTExt8::find_nn(RRTNode const& t, KdNode const* const ref, unsigned int lvl)
  32 {
  33         if (ref == nullptr) {
  34                 return;
  35         }
  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);
  40         }
  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);
  45                 }
  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);
  50                 }
  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);
  55                 }
  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);
  60                 }
  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);
  65                 }
  66         } else {
  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);
  70                 }
  71         }
  72 }
  73
  74 void
  75 RRTExt8::find_nv(RRTNode const& t, KdNode const* const ref, unsigned int lvl)
  76 {
  77         if (ref == nullptr) {
  78                 return;
  79         }
  80         if (this->cost_search(*ref->node, t) < this->cost_) {
  81                 this->nv_.push_back(ref->node);
  82         }
  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);
  87                 }
  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);
  92                 }
  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);
  97                 }
  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);
 102                 }
 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);
 107                 }
 108         } else {
 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);
 112                 }
 113         }
 114 }
 115
 116 RRTExt8::RRTExt8() : RRTS()
 117 {
 118         this->kdnodes_.reserve(this->nodes_.capacity());
 119         this->store(&this->nodes_.front(), this->kdroot_, 0);
 120 }
 121
 122 void
 123 RRTExt8::reset()
 124 {
 125         RRTS::reset();
 126         this->kdnodes_.clear();
 127         this->kdroot_ = nullptr;
 128         this->store(&this->nodes_.front(), this->kdroot_, 0);
 129 }
 130
 131 void
 132 RRTExt8::store(RRTNode n)
 133 {
 134         RRTS::store(n);
 135         this->store(&this->nodes_.back(), this->kdroot_, 0);
 136 }
 137
 138 void
 139 RRTExt8::find_nn(RRTNode const& t)
 140 {
 141         this->nn_ = &this->nodes_.front();
 142         this->cost_ = this->cost_search(*this->nn_, t);
 143         this->find_nn(t, this->kdroot_, 0);
 144 }
 145
 146 void
 147 RRTExt8::find_nv(RRTNode const& t)
 148 {
 149         this->nv_.clear();
 150         this->cost_ = this->min_gamma_eta();
 151         this->find_nv(t, this->kdroot_, 0);
 152 }
 153
 154 } // namespace rrts