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

diff --git a/src/psp.cc b/src/psp.cc
index 23f6e799a2ac1fab688c8dbb1e4e900d691e1e22..d6ce9238c21d4d980a2fcb5b079f292e42180e2b 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -391,7 +391,17 @@ void PSPlanner::fe_parallel()
                                 );
                         } else if (ccl_lr >= ccl_p1) {
                                 // in parking slot
-                                // TODO (ccl, lr) x (p1, p2)
+                                auto cli1 = ::intersect(
+                                        cclx, ccly, ccl_lr,
+                                        this->ps().x1(), this->ps().y1(),
+                                        this->ps().x2(), this->ps().y2()
+                                );
+                                double a1 = smaller_angle_of_two(
+                                        this->cc().lrx(), this->cc().lry(),
+                                        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(),