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 1")
   8 {
   9         RRTNode n1;
  10         RRTNode n2;
  11         RRTExt1 e1;
  12         // Test Matej's heuristics.
  13         n2.h(M_PI);
  14         e1.next();
  15         WVPASSEQ_DOUBLE(e1.cost_search(n1, n2), M_PI * n1.mtr(), 0.00001);
  16         // Test Reeds and Shepp path length.
  17         n2.h(0.3);
  18         double q0[] = {n1.x(), n1.y(), n1.h()};
  19         double q1[] = {n2.x(), n2.y(), n2.h()};
  20         ReedsSheppStateSpace rsss(n1.mtr());
  21         WVPASSEQ_DOUBLE(e1.cost_build(n1, n2), rsss.distance(q0, q1), 0.00001);
  22 }
  23
  24 WVTEST_MAIN("RRT extensions 2")
  25 {
  26         RRTExt2 rrts;
  27         WVPASSEQ_DOUBLE(cc(rrts.nodes().front()), 0, 0.00001);
  28         WVPASSEQ(rrts.nodes().size(), 1);
  29         rrts.next();
  30         WVPASSLT(1, rrts.nodes().size());
  31         WVPASSEQ(rrts.samples().size(), 1);
  32         rrts.goals().push_back(BicycleCar());
  33         rrts.goals().back().x(10);
  34         rrts.goals().back().y(10);
  35         rrts.goals().back().h(0);
  36         WVPASSEQ(rrts.goals().size(), 1);
  37         rrts.set_sample(0, 10, 0, 10, 0, 2 * M_PI);
  38         Obstacle o;
  39         o.poly().push_back(std::make_tuple(5, 5));
  40         o.poly().push_back(std::make_tuple(6, 5));
  41         o.poly().push_back(std::make_tuple(6, 6));
  42         o.poly().push_back(std::make_tuple(5, 6));
  43         o.poly().push_back(std::make_tuple(5, 5));
  44         rrts.obstacles().push_back(o);
  45         rrts.init_solid();
  46         while (rrts.next()) {}
  47         rrts.deinit_solid();
  48         WVPASS(rrts.path().size() > 0);
  49         WVPASS(
  50                 rrts.nodes().size() > 0
  51                 && rrts.path().size() > 0
  52                 && &rrts.nodes().front() == rrts.path().front()
  53         );
  54         WVPASS(
  55                 rrts.goals().size() > 0
  56                 && rrts.path().size() > 0
  57                 && &rrts.goals().front() == rrts.path().back()
  58         );
  59 }