namespace rrts {
+class RRTExt16 : public virtual RRTS {
+private:
+ void steer(RRTNode const& f, RRTNode const& t);
+};
+
class RRTExt15 : public virtual RRTS {
private:
std::vector<double> log_path_cost_;
--- /dev/null
+#include "reeds_shepp.h"
+#include "rrtext.hh"
+
+namespace rrts {
+
+static int
+cb_steer(double q[4], void *w)
+{
+ std::vector<RRTNode>* st = (std::vector<RRTNode>*) w;
+ st->push_back(RRTNode());
+ st->back().x(q[0]);
+ st->back().y(q[1]);
+ st->back().h(q[2]);
+ st->back().sp(q[3]);
+ return 0;
+}
+
+void
+RRTExt16::steer(RRTNode const& f, RRTNode const& t)
+{
+ this->steered_.clear();
+ double q0[] = {f.x(), f.y(), f.h()};
+ double q1[] = {t.x(), t.y(), t.h()};
+ ReedsSheppStateSpace rsss(this->bc_.mtr());
+ rsss.sample(q0, q1, this->eta_, cb_steer, &this->steered_);
+}
+
+} // namespace rrts