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