Add test for inside function

diff --git a/ut/psp.t.cc b/ut/psp.t.cc
index 5674806656124d351ba8785cab2f09c712413387..13bfde772ce8f06e88eaf6831952d6576a6c2de2 100644 (file)
--- a/ut/psp.t.cc
+++ b/ut/psp.t.cc
@@ -24,6 +24,16 @@ WVTEST_MAIN("parallel parking slot planner")
         WVPASS(psp.forward());
         WVPASSEQ_DOUBLE(psp.ps().heading(), psp.gc().h(), 0.00001);
         WVPASS(psp.parked());
+        std::vector<std::tuple<double, double>> slot;
+        slot.push_back(std::make_tuple(psp.ps().x1(), psp.ps().y1()));
+        slot.push_back(std::make_tuple(psp.ps().x2(), psp.ps().y2()));
+        slot.push_back(std::make_tuple(psp.ps().x3(), psp.ps().y3()));
+        slot.push_back(std::make_tuple(psp.ps().x4(), psp.ps().y4()));
+        WVPASS(inside(psp.gc().x(), psp.gc().y(), slot));
+        WVPASS(inside(psp.gc().lfx(), psp.gc().lfy(), slot));
+        WVPASS(inside(psp.gc().lrx(), psp.gc().lry(), slot));
+        WVPASS(inside(psp.gc().rrx(), psp.gc().rry(), slot));
+        WVPASS(inside(psp.gc().rfx(), psp.gc().rfy(), slot));
 
         // entry point found by reverse
         WVPASS(!psp.left());