ut/psp.t.cc

   1 #include <cmath>
   2 #include "wvtest.h"
   3
   4 #include "psp.h"
   5
   6 WVTEST_MAIN("parking slot planner basic test")
   7 {
   8         PSPlanner psp;
   9         psp.ps().border(3, 3, 5, 3, 5, 8, 3, 8);
  10         psp.gc().x(4);
  11         psp.gc().y(4);
  12         psp.gc().h(M_PI / 2);
  13         psp.gc().mtr(10);
  14         psp.gc().wb(2);
  15         psp.gc().w(1);
  16         psp.gc().l(3);
  17         psp.gc().he(1.5);
  18         psp.gc().df(2 + 0.5);
  19         psp.gc().dr(0.5);
  20         psp.cc() = BicycleCar(psp.gc());
  21
  22         // init orientation
  23         WVPASS(!psp.collide());
  24         WVPASSEQ_DOUBLE(psp.ps().heading(), psp.gc().h(), 0.00001);
  25
  26         // collide
  27         auto tmpi1 = intersect(1, 1, 3, 3, 1, 3, 3, 1);
  28         WVPASS(std::get<0>(tmpi1));
  29         WVPASSEQ_DOUBLE(std::get<1>(tmpi1), 2, 0.00001);
  30         WVPASSEQ_DOUBLE(std::get<2>(tmpi1), 2, 0.00001);
  31         auto tmpi2 = intersect(1, 1, 1, 3, 3, 1, 3, 3);
  32         WVPASS(!std::get<0>(tmpi2));
  33         PSPlanner tpsp;
  34         tpsp.ps().border(2, 3, 4, 3, 4, 8, 2, 8);
  35         tpsp.gc() = BicycleCar(psp.gc());
  36         tpsp.cc() = BicycleCar(tpsp.gc());
  37         WVPASS(tpsp.collide());
  38 }