ut/rrtext.t.cc

   1 #include <cmath>
   2 #include "wvtest.h"
   3
   4 #include "rrtext.h"
   5 #include "reeds_shepp.h"
   6
   7 WVTEST_MAIN("RRT extensions")
   8 {
   9         RRTNode n1;
  10         RRTNode n2;
  11         RRTExt1 e1;
  12         BicycleCar bc;
  13         e1.goals().push_back(RRTNode());
  14         // Test Matej's heuristics.
  15         n2.h(M_PI);
  16         e1.next();
  17         WVPASSEQ_DOUBLE(e1.cost_search(n1, n2), M_PI * bc.mtr(), 0.00001);
  18         // Test Reeds and Shepp path length.
  19         n2.h(0.3);
  20         double q0[] = {n1.x(), n1.y(), n1.h()};
  21         double q1[] = {n2.x(), n2.y(), n2.h()};
  22         ReedsSheppStateSpace rsss(bc.mtr());
  23         WVPASSEQ_DOUBLE(e1.cost_build(n1, n2), rsss.distance(q0, q1), 0.00001);
  24 }