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         e1.goals().push_back(RRTNode());
  13         // Test Matej's heuristics.
  14         n2.h(M_PI);
  15         e1.next();
  16         WVPASSEQ_DOUBLE(e1.cost_search(n1, n2), M_PI * n1.mtr(), 0.00001);
  17         // Test Reeds and Shepp path length.
  18         n2.h(0.3);
  19         double q0[] = {n1.x(), n1.y(), n1.h()};
  20         double q1[] = {n2.x(), n2.y(), n2.h()};
  21         ReedsSheppStateSpace rsss(n1.mtr());
  22         WVPASSEQ_DOUBLE(e1.cost_build(n1, n2), rsss.distance(q0, q1), 0.00001);
  23 }