src/rrtext14.cc

   1 #include "rrtext.hh"
   2
   3 namespace rrts {
   4
   5 RRTNode
   6 RRTExt14::sample()
   7 {
   8         if (this->circle_r_ == 0.0) {
   9                 RRTNode& f = this->nodes_.front();
  10                 RRTNode& g = this->goal_;
  11                 double dx = g.x() - f.x();
  12                 double dy = g.y() - f.y();
  13                 this->circle_r_ = sqrt(dx * dx + dy * dy);
  14                 this->circle_c_.x((f.x() + g.x()) / 2.0);
  15                 this->circle_c_.y((f.y() + g.y()) / 2.0);
  16                 return this->goal_;
  17         }
  18         double r = this->circle_r_ * sqrt(this->udr_(this->gen_));
  19         double theta = this->udt_(this->gen_);
  20         RRTNode rs;
  21         rs.x(this->circle_c_.x() + r * cos(theta));
  22         rs.y(this->circle_c_.y() + r * sin(theta));
  23         rs.h(this->udh_(this->gen_));
  24         return rs;
  25 }
  26
  27 RRTExt14::RRTExt14() : RRTS()
  28 {
  29         this->udr_ = std::uniform_real_distribution<double>(0.0, 1.0);
  30         this->udt_ = std::uniform_real_distribution<double>(0.0, 2.0 * M_PI);
  31         this->udh_ = std::uniform_real_distribution<double>(0.0, 2.0 * M_PI);
  32 }
  33
  34 void
  35 RRTExt14::reset()
  36 {
  37         RRTS::reset();
  38         this->circle_r_ = 0.0;
  39 }
  40
  41 } // namespace rrts