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 99;
  28         if (n.x() <= this->x_min_)
  29                 return 0;
  30         return (unsigned int) (floor(n.x() - this->x_min_) / ETA);
  31 }
  32
  33 unsigned int RRTExt4::yi(RRTNode n)
  34 {
  35         if (n.y() > this->y_max_)
  36                 return 99;
  37         if (n.y() <= this->y_min_)
  38                 return 0;
  39         return (unsigned int) (floor(n.y() - this->y_min_) / ETA);
  40 }
  41
  42 // API
  43 void RRTExt4::init()
  44 {
  45         this->x_min_ = this->nodes().back().x() - 50 * ETA;
  46         this->x_max_ = this->nodes().back().x() + 50 * ETA;
  47         this->y_min_ = this->nodes().back().y() - 50 * ETA;
  48         this->y_max_ = this->nodes().back().y() + 50 * ETA;
  49 }
  50
  51 void RRTExt4::store_node(RRTNode n)
  52 {
  53         RRTS::store_node(n);
  54         RRTNode *sn = &this->nodes().back();
  55         this->grid_[this->xi(n)][this->yi(n)].store_node(sn);
  56 }
  57
  58 RRTNode *RRTExt4::nn(RRTNode &t)
  59 {
  60         RRTNode *nn = &this->nodes().front();
  61         unsigned int txi = this->xi(t);
  62         unsigned int tyi = this->yi(t);
  63         unsigned int l = 0;
  64         while (this->cost_search(*nn, t) > l * ETA) {
  65                 int xi_min = txi - l;
  66                 if (xi_min < 0)
  67                         xi_min = 0;
  68                 int xi_max = txi + l;
  69                 if (xi_max > 99)
  70                         xi_max = 99;
  71                 int yi_min = tyi - l;
  72                 if (yi_min < 0)
  73                         yi_min = 0;
  74                 int yi_max = tyi + l;
  75                 if (yi_max > 99)
  76                         yi_max = 99;
  77                 for (int xi = xi_min; xi <= xi_max; xi++) {
  78                         this->grid_[xi][yi_max].nn(&t, &nn, this);
  79                 }
  80                 for (int xi = xi_min; xi <= xi_max; xi++) {
  81                         this->grid_[xi][yi_min].nn(&t, &nn, this);
  82                 }
  83                 for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
  84                         this->grid_[xi_min][yi].nn(&t, &nn, this);
  85                 }
  86                 for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
  87                         this->grid_[xi_max][yi].nn(&t, &nn, this);
  88                 }
  89                 l++;
  90         }
  91         return nn;
  92 }
  93
  94 std::vector<RRTNode *> RRTExt4::nv(RRTNode &t)
  95 {
  96         std::vector<RRTNode *> nv;
  97         unsigned int txi = this->xi(t);
  98         unsigned int tyi = this->yi(t);
  99         unsigned int l = 0;
 100         double cost = std::min(GAMMA(this->nodes().size()), ETA);
 101         for (auto f: this->grid_[txi][tyi].nodes())
 102                 if (this->cost_search(*f, t) < cost)
 103                         nv.push_back(f);
 104         return nv;
 105 }