rrts/src/rrtext4.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 void RRTExt4::Cell::nn(RRTNode *t, RRTNode **nn, RRTS *p)
  10 {
  11         double cost = p->cost_search(**nn, *t);
  12         for (auto f: this->nodes()) {
  13                 if (p->cost_search(*f, *t) < cost) {
  14                         *nn = f;
  15                         cost = p->cost_search(*f, *t);
  16                 }
  17         }
  18 }
  19
  20 void RRTExt4::Cell::store_node(RRTNode *n)
  21 {
  22         this->nodes_.push_back(n);
  23         this->changed_ = true;
  24 }
  25
  26 RRTExt4::Cell::Cell()
  27 {
  28 }
  29
  30 unsigned int RRTExt4::xi(RRTNode n)
  31 {
  32         if (n.x() >= this->x_max_)
  33                 return GRID_MAX_XI - 1;
  34         if (n.x() <= this->x_min_)
  35                 return 0;
  36         return (unsigned int) (floor(n.x() - this->x_min_) / GRID);
  37 }
  38
  39 unsigned int RRTExt4::yi(RRTNode n)
  40 {
  41         if (n.y() > this->y_max_)
  42                 return GRID_MAX_YI - 1;
  43         if (n.y() <= this->y_min_)
  44                 return 0;
  45         return (unsigned int) (floor(n.y() - this->y_min_) / GRID);
  46 }
  47
  48 // API
  49 void RRTExt4::init()
  50 {
  51         this->x_min_ = this->nodes().back().x() - GRID_WIDTH / 2;
  52         this->x_max_ = this->nodes().back().x() + GRID_WIDTH / 2;
  53         this->y_min_ = this->nodes().back().y() - GRID_HEIGHT / 2;
  54         this->y_max_ = this->nodes().back().y() + GRID_HEIGHT / 2;
  55 }
  56
  57 void RRTExt4::deinit()
  58 {
  59         for (unsigned int i = 0; i < GRID_MAX_XI; i++)
  60                 for (unsigned int j = 0; j < GRID_MAX_YI; j++)
  61                         this->grid_[i][j].nodes().clear();
  62 }
  63
  64 void RRTExt4::store_node(RRTNode n)
  65 {
  66         RRTS::store_node(n);
  67         RRTNode *sn = &this->nodes().back();
  68         this->grid_[this->xi(n)][this->yi(n)].store_node(sn);
  69 }
  70
  71 RRTNode *RRTExt4::nn(RRTNode &t)
  72 {
  73         RRTNode *nn = &this->nodes().front();
  74         unsigned int txi = this->xi(t);
  75         unsigned int tyi = this->yi(t);
  76         unsigned int l = 0;
  77         while (this->cost_search(*nn, t) > l * GRID) {
  78                 int xi_min = txi - l;
  79                 if (xi_min < 0)
  80                         xi_min = 0;
  81                 int xi_max = txi + l;
  82                 if (xi_max > GRID_MAX_XI - 1)
  83                         xi_max = GRID_MAX_XI - 1;
  84                 int yi_min = tyi - l;
  85                 if (yi_min < 0)
  86                         yi_min = 0;
  87                 int yi_max = tyi + l;
  88                 if (yi_max > GRID_MAX_YI - 1)
  89                         yi_max = GRID_MAX_YI - 1;
  90                 for (int xi = xi_min; xi <= xi_max; xi++) {
  91                         this->grid_[xi][yi_max].nn(&t, &nn, this);
  92                 }
  93                 for (int xi = xi_min; xi <= xi_max; xi++) {
  94                         this->grid_[xi][yi_min].nn(&t, &nn, this);
  95                 }
  96                 for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
  97                         this->grid_[xi_min][yi].nn(&t, &nn, this);
  98                 }
  99                 for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
 100                         this->grid_[xi_max][yi].nn(&t, &nn, this);
 101                 }
 102                 l++;
 103         }
 104         return nn;
 105 }
 106
 107 std::vector<RRTNode *> RRTExt4::nv(RRTNode &t)
 108 {
 109         std::vector<RRTNode *> nv;
 110         unsigned int txi = this->xi(t);
 111         unsigned int tyi = this->yi(t);
 112         unsigned int l = 0;
 113         double cost = std::min(GAMMA(this->nodes().size()), ETA);
 114         for (auto f: this->grid_[txi][tyi].nodes())
 115                 if (this->cost_search(*f, t) < cost)
 116                         nv.push_back(f);
 117         return nv;
 118 }