2 #include "reeds_shepp.h"
4 double RRTExt6::cost_build(RRTNode &f, RRTNode &t)
6 double q0[] = {f.x(), f.y(), f.h()};
7 double q1[] = {t.x(), t.y(), t.h()};
8 ReedsSheppStateSpace rsss(this->bc.mtr());
9 return rsss.distance(q0, q1);
12 double RRTExt6::cost_search(RRTNode &f, RRTNode &t)
14 double q0[] = {f.x(), f.y(), f.h()};
15 double q1[] = {t.x(), t.y(), t.h()};
16 ReedsSheppStateSpace rsss(this->bc.mtr());
17 return rsss.distance(q0, q1);