+#include "rrtext.hh"
+
+namespace rrts {
+
+RRTNode
+RRTExt14::sample()
+{
+ if (this->circle_r_ == 0.0) {
+ RRTNode& f = this->nodes_.front();
+ RRTNode& g = this->goal_;
+ double dx = g.x() - f.x();
+ double dy = g.y() - f.y();
+ this->circle_r_ = sqrt(dx * dx + dy * dy);
+ this->circle_c_.x((f.x() + g.x()) / 2.0);
+ this->circle_c_.y((f.y() + g.y()) / 2.0);
+ return this->goal_;
+ }
+ double r = this->circle_r_ * sqrt(this->udr_(this->gen_));
+ double theta = this->udt_(this->gen_);
+ RRTNode rs;
+ rs.x(this->circle_c_.x() + r * cos(theta));
+ rs.y(this->circle_c_.y() + r * sin(theta));
+ rs.h(this->udh_(this->gen_));
+ return rs;
+}
+
+RRTExt14::RRTExt14() : RRTS()
+{
+ this->udr_ = std::uniform_real_distribution<double>(0.0, 1.0);
+ this->udt_ = std::uniform_real_distribution<double>(0.0, 2.0 * M_PI);
+ this->udh_ = std::uniform_real_distribution<double>(0.0, 2.0 * M_PI);
+}
+
+void
+RRTExt14::reset()
+{
+ RRTS::reset();
+ this->circle_r_ = 0.0;
+}
+
+} // namespace rrts