double RRTExt5::cost_build(RRTNode &f, RRTNode &t)
{
- double q0[] = {f.x(), f.y(), f.h()};
- double q1[] = {t.x(), t.y(), t.h()};
- ReedsSheppStateSpace rsss(this->bc.mtr());
- return rsss.distance(q0, q1);
+ double q0[] = {f.x(), f.y(), f.h()};
+ double q1[] = {t.x(), t.y(), t.h()};
+ ReedsSheppStateSpace rsss(this->bc.mtr());
+ return rsss.distance(q0, q1);
}
double RRTExt5::cost_search(RRTNode &f, RRTNode &t)
{
- double cost = 0;
- cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
- return cost;
+ double cost = 0;
+ cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
+ return cost;
}