ut/rrts.t.cc

   1 #include <cmath>
   2 #include "wvtest.h"
   3
   4 #include "rrts.h"
   5
   6 WVTEST_MAIN("RRT node basic tests")
   7 {
   8         RRTNode n1;
   9         RRTNode n2;
  10 }
  11
  12 WVTEST_MAIN("RRT* basic tests")
  13 {
  14         RRTS rrts;
  15         rrts.goals().push_back(RRTNode());
  16         rrts.goals().back().x(10);
  17         rrts.goals().back().y(10);
  18         rrts.goals().back().h(0);
  19         WVPASSEQ_DOUBLE(cc(rrts.nodes().front()), 0, 0.00001);
  20         WVPASSEQ(rrts.nodes().size(), 1);
  21         rrts.next();
  22         WVPASSLT(1, rrts.nodes().size());
  23         WVPASSEQ(rrts.samples().size(), 1);
  24         WVPASSLT(0, rrts.goals().size());
  25         rrts.set_sample(0, 10, 0, 10, 0, 2 * M_PI);
  26         Obstacle o;
  27         o.poly().push_back(std::make_tuple(5, 5));
  28         o.poly().push_back(std::make_tuple(6, 5));
  29         o.poly().push_back(std::make_tuple(6, 6));
  30         o.poly().push_back(std::make_tuple(5, 6));
  31         o.poly().push_back(std::make_tuple(5, 5));
  32         rrts.obstacles().push_back(o);
  33         while (rrts.next()) {}
  34         WVPASS(rrts.path().size() > 0);
  35         WVPASS(
  36                 rrts.nodes().size() > 0
  37                 && rrts.path().size() > 0
  38                 && &rrts.nodes().front() == rrts.path().front()
  39         );
  40         WVPASS(
  41                 rrts.goals().size() > 0
  42                 && rrts.path().size() > 0
  43                 && &rrts.goals().front() == rrts.path().back()
  44         );
  45 }