Add guess goal car declaration, ut

[hubacji1/psp.git] / ut / psp.t.cc

diff --git a/ut/psp.t.cc b/ut/psp.t.cc
index 13bfde772ce8f06e88eaf6831952d6576a6c2de2..72aefa49cca54978ad8ad22c73530c11bd231ccb 100644 (file)
--- a/ut/psp.t.cc
+++ b/ut/psp.t.cc
@@ -7,9 +7,6 @@ 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);
@@ -17,6 +14,7 @@ WVTEST_MAIN("parallel parking slot planner")
         psp.gc().he(1.5);
         psp.gc().df(2 + 0.5);
         psp.gc().dr(0.5);
+        psp.guess_gc();
         psp.cc() = BicycleCar(psp.gc());
 
         // init orientation
@@ -35,41 +33,34 @@ WVTEST_MAIN("parallel parking slot planner")
         WVPASS(inside(psp.gc().rrx(), psp.gc().rry(), slot));
         WVPASS(inside(psp.gc().rfx(), psp.gc().rfy(), slot));
 
+        // init collide
+        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());
+
         // entry point found by reverse
         WVPASS(!psp.left());
         psp.fer();
         WVPASS(psp.left());
+        WVPASS(psp.parked());
 
         // find entry
         psp.cc() = BicycleCar(psp.gc());
         WVPASS(!psp.left());
         psp.fe();
         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());
+        WVPASS(psp.parked());
 }
 
 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);
@@ -77,6 +68,7 @@ WVTEST_MAIN("backward perpendicullar parking slot planner")
         psp.gc().he(1.5);
         psp.gc().df(2 + 0.5);
         psp.gc().dr(0.5);
+        psp.guess_gc();
         psp.cc() = BicycleCar(psp.gc());
 
         // init orientation
@@ -89,12 +81,14 @@ WVTEST_MAIN("backward perpendicullar parking slot planner")
         WVPASS(!psp.left());
         psp.fer();
         WVPASS(psp.left());
+        WVPASS(psp.parked());
 
         // find entry
         psp.cc() = BicycleCar(psp.gc());
         WVPASS(!psp.left());
         psp.fe();
         WVPASS(psp.left());
+        WVPASS(psp.parked());
 }
 
 WVTEST_MAIN("forward perpendicullar parking slot planner")
@@ -123,10 +117,12 @@ WVTEST_MAIN("forward perpendicullar parking slot planner")
         WVPASS(!psp.left());
         psp.fer();
         WVPASS(psp.left());
+        WVPASS(psp.parked());
 
         // find entry
         psp.cc() = BicycleCar(psp.gc());
         WVPASS(!psp.left());
         psp.fe();
         WVPASS(psp.left());
+        WVPASS(psp.parked());
 }