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