src/rrtext4.cc

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