src/rrtext9.cc

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