2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
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);
24 double r = this->circle_r_ * sqrt(this->udr_(this->gen_));
25 double theta = this->udt_(this->gen_);
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_));
33 RRTExt14::RRTExt14() : RRTS()
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);
44 this->circle_r_ = 0.0;