Add (p1, p2) x (lr, rr) computation

diff --git a/src/psp.cc b/src/psp.cc
index d6ce9238c21d4d980a2fcb5b079f292e42180e2b..66794b9f3b87dae8a5aec30500c45d600bcf05be 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -377,7 +377,17 @@ void PSPlanner::fe_parallel()
                                 continue;
                         } else if (ccl_rr >= ccl_p1 && ccl_lr < ccl_p1) {
                                 // partially out of parking slot
-                                // TODO (p1, p2) x (lr, rr)
+                                auto cli1 = ::intersect(
+                                        cclx, ccly, ccl_p1,
+                                        this->cc().lrx(), this->cc().lry(),
+                                        this->cc().rrx(), this->cc().rry()
+                                );
+                                double a1 = ::smaller_angle_of_two(
+                                        this->ps().x1(), this->ps().y1(),
+                                        cclx, ccly,
+                                        std::get<1>(cli1), std::get<2>(cli1),
+                                        std::get<3>(cli1), std::get<4>(cli1)
+                                );
                                 auto cli2 = ::intersect(
                                         cclx, ccly, ccl_rr,
                                         this->ps().x2(), this->ps().y2(),