src/rrtext7.cc

   1 /*
   2  * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 #include "rrtext.h"
   8
   9 RRTExt7::KdNode::KdNode(RRTNode *n)
  10         : node_(n)
  11         , left_(nullptr)
  12         , right_(nullptr)
  13 {
  14 }
  15
  16 void RRTExt7::delete_kd_nodes(KdNode *n)
  17 {
  18         if (!n)
  19                 return;
  20         if (n->left() != nullptr)
  21                 delete_kd_nodes(n->left());
  22         if (n->right() != nullptr)
  23                 delete_kd_nodes(n->right());
  24         delete n;
  25 }
  26
  27 void RRTExt7::store_node(RRTNode *n, KdNode *&r, int l)
  28 {
  29         if (r == nullptr)
  30                 r = new KdNode(n);
  31         else if (l % 2 == 0 && n->x() < r->node()->x())
  32                 store_node(n, r->left(), l + 1);
  33         else if (l % 2 == 0)
  34                 store_node(n, r->right(), l + 1);
  35         else if (l % 2 == 1 && n->y() < r->node()->y())
  36                 store_node(n, r->left(), l + 1);
  37         else
  38                 store_node(n, r->right(), l + 1);
  39 }
  40
  41 void RRTExt7::nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d)
  42 {
  43         if (r == nullptr)
  44                 return;
  45         if (this->cost_search(*r->node(), t) < d) {
  46                 n = r->node();
  47                 d = this->cost_search(*r->node(), t);
  48         }
  49         if (l % 2 == 0 && t.x() < r->node()->x()) {
  50                 nn(n, t, r->left(), l + 1, d);
  51                 if (r->node()->x() - t.x() < d)
  52                         nn(n, t, r->right(), l + 1, d);
  53         } else if (l % 2 == 0) {
  54                 nn(n, t, r->right(), l + 1, d);
  55                 if (t.x() - r->node()->x() < d)
  56                         nn(n, t, r->left(), l + 1, d);
  57         } else if (l % 2 == 1 && t.y() < r->node()->y()) {
  58                 nn(n, t, r->left(), l + 1, d);
  59                 if (r->node()->y() - t.y() < d)
  60                         nn(n, t, r->right(), l + 1, d);
  61         } else {
  62                 nn(n, t, r->right(), l + 1, d);
  63                 if (t.y() - r->node()->y() < d)
  64                         nn(n, t, r->left(), l + 1, d);
  65         }
  66 }
  67
  68 // API
  69 void RRTExt7::init()
  70 {
  71 }
  72
  73 void RRTExt7::deinit()
  74 {
  75         this->delete_kd_nodes(this->kdroot_);
  76 }
  77
  78 void RRTExt7::store_node(RRTNode n)
  79 {
  80         RRTS::store_node(n);
  81         RRTNode *sn = &this->nodes().back();
  82         this->store_node(sn, this->kdroot_, 0);
  83 }
  84
  85 RRTNode *RRTExt7::nn(RRTNode &t)
  86 {
  87         RRTNode *n = &this->nodes().front();
  88         double d = 9999;
  89         this->nn(n, t, this->kdroot_, 0, d);
  90         return n;
  91 }
  92
  93 std::vector<RRTNode *> RRTExt7::nv(RRTNode &t)
  94 {
  95         std::vector<RRTNode *> nv;
  96         return nv;
  97 }