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);
18 double r = this->circle_r_ * sqrt(this->udr_(this->gen_));
19 double theta = this->udt_(this->gen_);
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_));
27 RRTExt14::RRTExt14() : RRTS()
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);
38 this->circle_r_ = 0.0;