src/rrtext9.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 RRTExt9::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 RRTExt9::Cell::store_node(RRTNode *n)
  21 {
  22         this->nodes_.push_back(n);
  23         this->changed_ = true;
  24 }
  25
  26 RRTExt9::Cell::Cell()
  27 {
  28 }
  29
  30 unsigned int RRTExt9::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 RRTExt9::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 unsigned int RRTExt9::hi(RRTNode n)
  49 {
  50         if (n.h() > this->h_max_)
  51                 return GRID_MAX_HI - 1;
  52         if (n.h() <= this->h_min_)
  53                 return 0;
  54         return (unsigned int) (n.h() * GRID_MAX_HI / (2 * M_PI));
  55 }
  56
  57 // API
  58 void RRTExt9::init()
  59 {
  60         this->x_min_ = this->nodes().back().x() - GRID_WIDTH / 2;
  61         this->x_max_ = this->nodes().back().x() + GRID_WIDTH / 2;
  62         this->y_min_ = this->nodes().back().y() - GRID_HEIGHT / 2;
  63         this->y_max_ = this->nodes().back().y() + GRID_HEIGHT / 2;
  64 }
  65
  66 void RRTExt9::deinit()
  67 {
  68         for (unsigned int i = 0; i < GRID_MAX_XI; i++)
  69                 for (unsigned int j = 0; j < GRID_MAX_YI; j++)
  70                         for (unsigned int k = 0; k < GRID_MAX_HI; k++)
  71                                 this->grid_[i][j][k].nodes().clear();
  72 }
  73
  74 void RRTExt9::store_node(RRTNode n)
  75 {
  76         RRTS::store_node(n);
  77         RRTNode *sn = &this->nodes().back();
  78         this->grid_[this->xi(n)][this->yi(n)][this->hi(n)].store_node(sn);
  79 }
  80
  81 RRTNode *RRTExt9::nn(RRTNode &t)
  82 {
  83         RRTNode *nn = &this->nodes().front();
  84         unsigned int txi = this->xi(t);
  85         unsigned int tyi = this->yi(t);
  86         unsigned int thi = this->hi(t);
  87         unsigned int l = 0;
  88         while (this->cost_search(*nn, t) > l * GRID) {
  89                 int xi_min = txi - l;
  90                 if (xi_min < 0)
  91                         xi_min = 0;
  92                 int xi_max = txi + l;
  93                 if (xi_max > GRID_MAX_XI - 1)
  94                         xi_max = GRID_MAX_XI - 1;
  95                 int yi_min = tyi - l;
  96                 if (yi_min < 0)
  97                         yi_min = 0;
  98                 int yi_max = tyi + l;
  99                 if (yi_max > GRID_MAX_YI - 1)
 100                         yi_max = GRID_MAX_YI - 1;
 101                 int hi_min = thi - l; // TODO respect kinematic constraints
 102                 if (hi_min < 0)
 103                         hi_min = 0;
 104                 int hi_max = thi + l;
 105                 if (hi_max > GRID_MAX_HI - 1)
 106                         hi_max = GRID_MAX_HI - 1;
 107                 for (int hi = hi_min; hi <= hi_max; hi++) {
 108                         for (int xi = xi_min; xi <= xi_max; xi++) {
 109                                 this->grid_[xi][yi_max][hi].nn(&t, &nn, this);
 110                         }
 111                         for (int xi = xi_min; xi <= xi_max; xi++) {
 112                                 this->grid_[xi][yi_min][hi].nn(&t, &nn, this);
 113                         }
 114                         for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
 115                                 this->grid_[xi_min][yi][hi].nn(&t, &nn, this);
 116                         }
 117                         for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
 118                                 this->grid_[xi_max][yi][hi].nn(&t, &nn, this);
 119                         }
 120                 }
 121                 l++;
 122         }
 123         return nn;
 124 }
 125
 126 std::vector<RRTNode *> RRTExt9::nv(RRTNode &t)
 127 {
 128         std::vector<RRTNode *> nv;
 129         unsigned int txi = this->xi(t);
 130         unsigned int tyi = this->yi(t);
 131         unsigned int thi = this->hi(t);
 132         unsigned int l = 0;
 133         double cost = std::min(GAMMA(this->nodes().size()), ETA);
 134         for (auto f: this->grid_[txi][tyi][thi].nodes())
 135                 if (this->cost_search(*f, t) < cost)
 136                         nv.push_back(f);
 137         return nv;
 138 }