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::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 RRTExt7::store_node(RRTNode *n, KdNode *&r, int l)
  22 {
  23         if (r == nullptr)
  24                 r = new KdNode(n);
  25         else if (l % 2 == 0 && n->x() < r->node()->x())
  26                 store_node(n, r->left(), l + 1);
  27         else if (l % 2 == 0)
  28                 store_node(n, r->right(), l + 1);
  29         else if (l % 2 == 1 && n->y() < r->node()->y())
  30                 store_node(n, r->left(), l + 1);
  31         else
  32                 store_node(n, r->right(), l + 1);
  33 }
  34
  35 void RRTExt7::nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d)
  36 {
  37         if (r == nullptr)
  38                 return;
  39         if (this->cost_search(*r->node(), t) < d) {
  40                 n = r->node();
  41                 d = this->cost_search(*r->node(), t);
  42         }
  43         if (l % 2 == 0 && t.x() < r->node()->x()) {
  44                 nn(n, t, r->left(), l + 1, d);
  45                 if (r->node()->x() - t.x() < d)
  46                         nn(n, t, r->right(), l + 1, d);
  47         } else if (l % 2 == 0) {
  48                 nn(n, t, r->right(), l + 1, d);
  49                 if (t.x() - r->node()->x() < d)
  50                         nn(n, t, r->left(), l + 1, d);
  51         } else if (l % 2 == 1 && t.y() < r->node()->y()) {
  52                 nn(n, t, r->left(), l + 1, d);
  53                 if (r->node()->y() - t.y() < d)
  54                         nn(n, t, r->right(), l + 1, d);
  55         } else {
  56                 nn(n, t, r->right(), l + 1, d);
  57                 if (t.y() - r->node()->y() < d)
  58                         nn(n, t, r->left(), l + 1, d);
  59         }
  60 }
  61
  62 // API
  63 void RRTExt7::init()
  64 {
  65 }
  66
  67 void RRTExt7::deinit()
  68 {
  69         this->delete_kd_nodes(this->kdroot_);
  70 }
  71
  72 void RRTExt7::store_node(RRTNode n)
  73 {
  74         RRTS::store_node(n);
  75         RRTNode *sn = &this->nodes().back();
  76         this->store_node(sn, this->kdroot_, 0);
  77 }
  78
  79 RRTNode *RRTExt7::nn(RRTNode &t)
  80 {
  81         RRTNode *n = &this->nodes().front();
  82         double d = 9999;
  83         this->nn(n, t, this->kdroot_, 0, d);
  84         return n;
  85 }
  86
  87 std::vector<RRTNode *> RRTExt7::nv(RRTNode &t)
  88 {
  89         std::vector<RRTNode *> nv;
  90         return nv;
  91 }