src/rrtext8.cc

   1 #include "rrtext.h"
   2
   3 RRTExt8::KdNode::KdNode(RRTNode *n)
   4         : node_(n)
   5         , left_(nullptr)
   6         , right_(nullptr)
   7 {
   8 }
   9
  10 void RRTExt8::delete_kd_nodes(KdNode *n)
  11 {
  12         if (n == nullptr)
  13                 return;
  14         this->delete_kd_nodes(n->left());
  15         this->delete_kd_nodes(n->right());
  16         delete n;
  17 }
  18
  19 void RRTExt8::store_node(RRTNode *n, KdNode *&r, int l)
  20 {
  21         if (r == nullptr)
  22                 r = new KdNode(n);
  23         else if (l % 3 == 0 && n->x() < r->node()->x())
  24                 store_node(n, r->left(), l + 1);
  25         else if (l % 3 == 0)
  26                 store_node(n, r->right(), l + 1);
  27         else if (l % 3 == 1 && n->y() < r->node()->y())
  28                 store_node(n, r->left(), l + 1);
  29         else if (l % 3 == 1)
  30                 store_node(n, r->right(), l + 1);
  31         else if (l % 3 == 2 && n->h() < r->node()->h())
  32                 store_node(n, r->left(), l + 1);
  33         else
  34                 store_node(n, r->right(), l + 1);
  35 }
  36
  37 void RRTExt8::nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d)
  38 {
  39         if (r == nullptr)
  40                 return;
  41         if (this->cost_search(*r->node(), t) < d) {
  42                 n = r->node();
  43                 d = this->cost_search(*r->node(), t);
  44         }
  45         if (l % 3 == 0 && t.x() < r->node()->x()) {
  46                 nn(n, t, r->left(), l + 1, d);
  47                 if (r->node()->x() - t.x() < d)
  48                         nn(n, t, r->right(), l + 1, d);
  49         } else if (l % 3 == 0) {
  50                 nn(n, t, r->right(), l + 1, d);
  51                 if (t.x() - r->node()->x() < d)
  52                         nn(n, t, r->left(), l + 1, d);
  53         } else if (l % 3 == 1 && t.y() < r->node()->y()) {
  54                 nn(n, t, r->left(), l + 1, d);
  55                 if (r->node()->y() - t.y() < d)
  56                         nn(n, t, r->right(), l + 1, d);
  57         } else if (l % 3 == 1) {
  58                 nn(n, t, r->right(), l + 1, d);
  59                 if (t.y() - r->node()->y() < d)
  60                         nn(n, t, r->left(), l + 1, d);
  61         } else if (l % 3 == 2 && t.h() < r->node()->h()) {
  62                 nn(n, t, r->left(), l + 1, d);
  63                 if (r->node()->h() - t.h() < d)
  64                         nn(n, t, r->right(), l + 1, d);
  65         } else {
  66                 nn(n, t, r->right(), l + 1, d);
  67                 if (t.h() - r->node()->h() < d)
  68                         nn(n, t, r->left(), l + 1, d);
  69         }
  70 }
  71
  72 void RRTExt8::nv(
  73         std::vector<RRTNode*>& n,
  74         RRTNode &t,
  75         KdNode *r,
  76         int l,
  77         double const& d
  78 )
  79 {
  80         if (r == nullptr)
  81                 return;
  82         if (this->cost_search(*r->node(), t) < d) {
  83                 n.push_back(r->node());
  84         }
  85         if (l % 3 == 0 && t.x() < r->node()->x()) {
  86                 this->nv(n, t, r->left(), l + 1, d);
  87                 if (r->node()->x() - t.x() < d)
  88                         this->nv(n, t, r->right(), l + 1, d);
  89         } else if (l % 3 == 0) {
  90                 this->nv(n, t, r->right(), l + 1, d);
  91                 if (t.x() - r->node()->x() < d)
  92                         this->nv(n, t, r->left(), l + 1, d);
  93         } else if (l % 3 == 1 && t.y() < r->node()->y()) {
  94                 this->nv(n, t, r->left(), l + 1, d);
  95                 if (r->node()->y() - t.y() < d)
  96                         this->nv(n, t, r->right(), l + 1, d);
  97         } else if (l % 3 == 1) {
  98                 this->nv(n, t, r->right(), l + 1, d);
  99                 if (t.y() - r->node()->y() < d)
 100                         this->nv(n, t, r->left(), l + 1, d);
 101         } else if (l % 3 == 2 && t.h() < r->node()->h()) {
 102                 this->nv(n, t, r->left(), l + 1, d);
 103                 if (r->node()->h() - t.h() < d)
 104                         this->nv(n, t, r->right(), l + 1, d);
 105         } else {
 106                 this->nv(n, t, r->right(), l + 1, d);
 107                 if (t.h() - r->node()->h() < d)
 108                         this->nv(n, t, r->left(), l + 1, d);
 109         }
 110 }
 111
 112 // API
 113 void RRTExt8::init()
 114 {
 115 }
 116
 117 void RRTExt8::reset()
 118 {
 119         RRTS::reset();
 120         this->delete_kd_nodes();
 121 }
 122
 123 void RRTExt8::deinit()
 124 {
 125         this->delete_kd_nodes(this->kdroot_);
 126 }
 127
 128 void RRTExt8::store_node(RRTNode n)
 129 {
 130         RRTS::store_node(n);
 131         RRTNode *sn = &this->nodes().back();
 132         this->store_node(sn, this->kdroot_, 0);
 133 }
 134
 135 RRTNode *RRTExt8::nn(RRTNode &t)
 136 {
 137         RRTNode *n = &this->nodes().front();
 138         double d = 9999;
 139         this->nn(n, t, this->kdroot_, 0, d);
 140         return n;
 141 }
 142
 143 std::vector<RRTNode *> RRTExt8::nv(RRTNode &t)
 144 {
 145         double cost = std::min(GAMMA(this->nodes().size()), 0.5);
 146         std::vector<RRTNode *> n;
 147         this->nv(n, t, this->kdroot_, 0, cost);
 148         return n;
 149 }