2 #include "reeds_shepp.h"
7 RRTExt10::cost_build(RRTNode const& f, RRTNode const& t) const
9 double q0[] = {f.x(), f.y(), f.h()};
10 double q1[] = {t.x(), t.y(), t.h()};
11 ReedsSheppStateSpace rsss(this->bc_.mtr());
12 return rsss.distance(q0, q1);
16 RRTExt10::cost_search(RRTNode const& f, RRTNode const& t) const
18 return f.edist(t) + std::abs(t.h() - f.h());