Add line segment intersection skeleton, ut

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

diff --git a/ut/psp.t.cc b/ut/psp.t.cc
index 6ee13312cf8690bebaff3018cdf17c2fa61139b3..277e118885de3c5eeeaa6e30716c18511489c8b2 100644 (file)
--- a/ut/psp.t.cc
+++ b/ut/psp.t.cc
@@ -23,6 +23,12 @@ WVTEST_MAIN("parking slot planner basic test")
         WVPASSEQ_DOUBLE(psp.ps().heading(), psp.gc().h(), 0.00001);
 
         // 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());