6 WVTEST_MAIN("parallel parking slot planner")
9 psp.ps().border(3, 3, 5, 3, 5, 8, 3, 8);
20 psp.cc() = BicycleCar(psp.gc());
23 WVPASS(!psp.collide());
24 WVPASS(psp.forward());
25 WVPASSEQ_DOUBLE(psp.ps().heading(), psp.gc().h(), 0.00001);
27 std::vector<std::tuple<double, double>> slot;
28 slot.push_back(std::make_tuple(psp.ps().x1(), psp.ps().y1()));
29 slot.push_back(std::make_tuple(psp.ps().x2(), psp.ps().y2()));
30 slot.push_back(std::make_tuple(psp.ps().x3(), psp.ps().y3()));
31 slot.push_back(std::make_tuple(psp.ps().x4(), psp.ps().y4()));
32 WVPASS(inside(psp.gc().x(), psp.gc().y(), slot));
33 WVPASS(inside(psp.gc().lfx(), psp.gc().lfy(), slot));
34 WVPASS(inside(psp.gc().lrx(), psp.gc().lry(), slot));
35 WVPASS(inside(psp.gc().rrx(), psp.gc().rry(), slot));
36 WVPASS(inside(psp.gc().rfx(), psp.gc().rfy(), slot));
38 // entry point found by reverse
44 psp.cc() = BicycleCar(psp.gc());
50 auto tmpi1 = intersect(1, 1, 3, 3, 1, 3, 3, 1);
51 WVPASS(std::get<0>(tmpi1));
52 WVPASSEQ_DOUBLE(std::get<1>(tmpi1), 2, 0.00001);
53 WVPASSEQ_DOUBLE(std::get<2>(tmpi1), 2, 0.00001);
54 auto tmpi2 = intersect(1, 1, 1, 3, 3, 1, 3, 3);
55 WVPASS(!std::get<0>(tmpi2));
57 tpsp.ps().border(2, 3, 4, 3, 4, 8, 2, 8);
58 tpsp.gc() = BicycleCar(psp.gc());
59 tpsp.cc() = BicycleCar(tpsp.gc());
60 WVPASS(tpsp.collide());
62 tpsp.ps().border(3, 4.1, 3, 2.1, 8, 2.1, 8, 4.1);
66 WVTEST_MAIN("backward perpendicullar parking slot planner")
69 psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
80 psp.cc() = BicycleCar(psp.gc());
83 WVPASS(!psp.collide());
84 WVPASS(!psp.forward());
85 WVPASSEQ_DOUBLE(psp.ps().heading() + M_PI / 2, psp.gc().h(), 0.00001);
88 // entry point found by reverse
94 psp.cc() = BicycleCar(psp.gc());
100 WVTEST_MAIN("forward perpendicullar parking slot planner")
103 psp.ps().border(3, 3, 8, 3, 8, 5, 3, 5);
112 psp.gc().df(2 + 0.5);
114 psp.cc() = BicycleCar(psp.gc());
117 WVPASS(!psp.collide());
118 WVPASS(psp.forward());
119 WVPASSEQ_DOUBLE(psp.ps().heading() - M_PI / 2, psp.gc().h(), 0.00001);
120 WVPASS(psp.parked());
122 // entry point found by reverse
128 psp.cc() = BicycleCar(psp.gc());