src/rrtext7.cc

   1 #include "rrtext.h"
   2
   3 RRTExt7::KdNode::KdNode(RRTNode *n)
   4         : node_(n)
   5         , left_(nullptr)
   6         , right_(nullptr)
   7 {
   8 }
   9
  10 void RRTExt7::store_node(RRTNode *n, KdNode *&r, int l)
  11 {
  12         if (r == nullptr)
  13                 r = new KdNode(n);
  14         else if (l % 2 == 0 && n->x() < r->node()->x())
  15                 store_node(n, r->left(), l + 1);
  16         else if (l % 2 == 0)
  17                 store_node(n, r->right(), l + 1);
  18         else if (l % 2 == 1 && n->y() < r->node()->y())
  19                 store_node(n, r->left(), l + 1);
  20         else
  21                 store_node(n, r->right(), l + 1);
  22 }
  23
  24 void RRTExt7::nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d)
  25 {
  26         if (r == nullptr)
  27                 return;
  28         if (this->cost_search(*r->node(), t) < d) {
  29                 n = r->node();
  30                 d = this->cost_search(*r->node(), t);
  31         }
  32         if (l % 2 == 0 && t.x() < r->node()->x()) {
  33                 nn(n, t, r->left(), l + 1, d);
  34                 if (r->node()->x() - t.x() < d)
  35                         nn(n, t, r->right(), l + 1, d);
  36         } else if (l % 2 == 0) {
  37                 nn(n, t, r->right(), l + 1, d);
  38                 if (t.x() - r->node()->x() < d)
  39                         nn(n, t, r->left(), l + 1, d);
  40         } else if (l % 2 == 1 && t.y() < r->node()->y()) {
  41                 nn(n, t, r->left(), l + 1, d);
  42                 if (r->node()->y() - t.y() < d)
  43                         nn(n, t, r->right(), l + 1, d);
  44         } else {
  45                 nn(n, t, r->right(), l + 1, d);
  46                 if (t.y() - r->node()->y() < d)
  47                         nn(n, t, r->left(), l + 1, d);
  48         }
  49 }
  50
  51 // API
  52 void RRTExt7::init()
  53 {
  54 }
  55
  56 void RRTExt7::deinit()
  57 {
  58 }
  59
  60 void RRTExt7::store_node(RRTNode n)
  61 {
  62         RRTS::store_node(n);
  63         RRTNode *sn = &this->nodes().back();
  64         this->store_node(sn, this->kdroot_, 0);
  65 }
  66
  67 RRTNode *RRTExt7::nn(RRTNode &t)
  68 {
  69         RRTNode *n = &this->nodes().front();
  70         double d = 9999;
  71         this->nn(n, t, this->kdroot_, 0, d);
  72         return n;
  73 }
  74
  75 std::vector<RRTNode *> RRTExt7::nv(RRTNode &t)
  76 {
  77         std::vector<RRTNode *> nv;
  78         return nv;
  79 }