#include "psp.h"
-WVTEST_MAIN("parallel parking slot planner")
-{
- PSPlanner psp;
- psp.ps().border(3, 3, 5, 3, 5, 8, 3, 8);
- psp.gc().x(4);
- psp.gc().y(4);
- psp.gc().h(M_PI / 2);
- psp.gc().mtr(10);
- psp.gc().wb(2);
- psp.gc().w(1);
- psp.gc().l(3);
- psp.gc().he(1.5);
- psp.gc().df(2 + 0.5);
- psp.gc().dr(0.5);
- psp.cc() = BicycleCar(psp.gc());
-
- // init orientation
- WVPASS(!psp.collide());
- WVPASS(psp.forward());
- WVPASSEQ_DOUBLE(psp.ps().heading(), psp.gc().h(), 0.00001);
-
- // entry point found by reverse
- WVPASS(!psp.left());
- psp.fer();
- WVPASS(psp.left());
-
- // collide
- auto tmpi1 = intersect(1, 1, 3, 3, 1, 3, 3, 1);
- WVPASS(std::get<0>(tmpi1));
- WVPASSEQ_DOUBLE(std::get<1>(tmpi1), 2, 0.00001);
- WVPASSEQ_DOUBLE(std::get<2>(tmpi1), 2, 0.00001);
- auto tmpi2 = intersect(1, 1, 1, 3, 3, 1, 3, 3);
- WVPASS(!std::get<0>(tmpi2));
- PSPlanner tpsp;
- tpsp.ps().border(2, 3, 4, 3, 4, 8, 2, 8);
- tpsp.gc() = BicycleCar(psp.gc());
- tpsp.cc() = BicycleCar(tpsp.gc());
- WVPASS(tpsp.collide());
- WVPASS(!tpsp.left());
- tpsp.ps().border(3, 4.1, 3, 2.1, 8, 2.1, 8, 4.1);
- WVPASS(tpsp.left());
-}
-
-WVTEST_MAIN("backward perpendicullar parking slot planner")
-{
- PSPlanner psp;
- psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
- psp.gc().x(7);
- psp.gc().y(4);
- psp.gc().h(M_PI);
- psp.gc().mtr(10);
- psp.gc().wb(2);
- psp.gc().w(1);
- psp.gc().l(3);
- psp.gc().he(1.5);
- psp.gc().df(2 + 0.5);
- psp.gc().dr(0.5);
- psp.cc() = BicycleCar(psp.gc());
-
- // init orientation
- WVPASS(!psp.collide());
- WVPASS(!psp.forward());
- WVPASSEQ_DOUBLE(psp.ps().heading() + M_PI / 2, psp.gc().h(), 0.00001);
-
- // entry point found by reverse
- WVPASS(!psp.left());
- psp.fer();
- WVPASS(psp.left());
-}
-
-WVTEST_MAIN("forward perpendicullar parking slot planner")
-{
- PSPlanner psp;
- psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
- psp.gc().x(4);
- psp.gc().y(4);
- psp.gc().h(0);
- psp.gc().mtr(10);
- psp.gc().wb(2);
- psp.gc().w(1);
- psp.gc().l(3);
- psp.gc().he(1.5);
- psp.gc().df(2 + 0.5);
- psp.gc().dr(0.5);
- psp.cc() = BicycleCar(psp.gc());
-
- // init orientation
- WVPASS(!psp.collide());
- WVPASS(psp.forward());
- WVPASSEQ_DOUBLE(psp.ps().heading() - M_PI / 2, psp.gc().h(), 0.00001);
-
- // entry point found by reverse
- WVPASS(!psp.left());
- psp.fer();
- WVPASS(psp.left());
+WVTEST_MAIN("pslot blank") {
}