RRTNode n1;
RRTNode n2;
RRTExt1 e1;
+ BicycleCar bc;
e1.goals().push_back(RRTNode());
// Test Matej's heuristics.
n2.h(M_PI);
e1.next();
- WVPASSEQ_DOUBLE(e1.cost_search(n1, n2), M_PI * n1.mtr(), 0.00001);
+ WVPASSEQ_DOUBLE(e1.cost_search(n1, n2), M_PI * bc.mtr(), 0.00001);
// Test Reeds and Shepp path length.
n2.h(0.3);
double q0[] = {n1.x(), n1.y(), n1.h()};
double q1[] = {n2.x(), n2.y(), n2.h()};
- ReedsSheppStateSpace rsss(n1.mtr());
+ ReedsSheppStateSpace rsss(bc.mtr());
WVPASSEQ_DOUBLE(e1.cost_build(n1, n2), rsss.distance(q0, q1), 0.00001);
}