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