ut/psp.t.cc

   1 #include <cmath>
   2 #include "wvtest.h"
   3
   4 #include "psp.h"
   5
   6 WVTEST_MAIN("parallel parking slot planner")
   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         WVPASS(psp.forward());
  25         WVPASSEQ_DOUBLE(psp.ps().heading(), psp.gc().h(), 0.00001);
  26
  27         // entry point found by reverse
  28         WVPASS(!psp.left());
  29         psp.fer();
  30         WVPASS(psp.left());
  31
  32         // collide
  33         auto tmpi1 = intersect(1, 1, 3, 3, 1, 3, 3, 1);
  34         WVPASS(std::get<0>(tmpi1));
  35         WVPASSEQ_DOUBLE(std::get<1>(tmpi1), 2, 0.00001);
  36         WVPASSEQ_DOUBLE(std::get<2>(tmpi1), 2, 0.00001);
  37         auto tmpi2 = intersect(1, 1, 1, 3, 3, 1, 3, 3);
  38         WVPASS(!std::get<0>(tmpi2));
  39         PSPlanner tpsp;
  40         tpsp.ps().border(2, 3, 4, 3, 4, 8, 2, 8);
  41         tpsp.gc() = BicycleCar(psp.gc());
  42         tpsp.cc() = BicycleCar(tpsp.gc());
  43         WVPASS(tpsp.collide());
  44         WVPASS(!tpsp.left());
  45         tpsp.ps().border(3, 4.1, 3, 2.1, 8, 2.1, 8, 4.1);
  46         WVPASS(tpsp.left());
  47 }
  48
  49 WVTEST_MAIN("backward perpendicullar parking slot planner")
  50 {
  51         PSPlanner psp;
  52         psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
  53         psp.gc().x(7);
  54         psp.gc().y(4);
  55         psp.gc().h(M_PI);
  56         psp.gc().mtr(10);
  57         psp.gc().wb(2);
  58         psp.gc().w(1);
  59         psp.gc().l(3);
  60         psp.gc().he(1.5);
  61         psp.gc().df(2 + 0.5);
  62         psp.gc().dr(0.5);
  63         psp.cc() = BicycleCar(psp.gc());
  64
  65         // init orientation
  66         WVPASS(!psp.collide());
  67         WVPASS(!psp.forward());
  68         WVPASSEQ_DOUBLE(psp.ps().heading() + M_PI / 2, psp.gc().h(), 0.00001);
  69
  70         // entry point found by reverse
  71         WVPASS(!psp.left());
  72         psp.fer();
  73         WVPASS(psp.left());
  74 }
  75
  76 WVTEST_MAIN("forward perpendicullar parking slot planner")
  77 {
  78         PSPlanner psp;
  79         psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
  80         psp.gc().x(4);
  81         psp.gc().y(4);
  82         psp.gc().h(0);
  83         psp.gc().mtr(10);
  84         psp.gc().wb(2);
  85         psp.gc().w(1);
  86         psp.gc().l(3);
  87         psp.gc().he(1.5);
  88         psp.gc().df(2 + 0.5);
  89         psp.gc().dr(0.5);
  90         psp.cc() = BicycleCar(psp.gc());
  91
  92         // init orientation
  93         WVPASS(!psp.collide());
  94         WVPASS(psp.forward());
  95         WVPASSEQ_DOUBLE(psp.ps().heading() - M_PI / 2, psp.gc().h(), 0.00001);
  96
  97         // entry point found by reverse
  98         WVPASS(!psp.left());
  99         psp.fer();
 100         WVPASS(psp.left());
 101 }