Add (ccl, rr) x (p2, p3) computation

diff --git a/src/psp.cc b/src/psp.cc
index 1dac5b6e7eba1b7a46607fbc51b5181e9296ebc5..12d897dfe3d1c66f82d6b013683a9feaeefea1fe 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -367,11 +367,41 @@ void PSPlanner::fe_parallel()
                         } else if (ccl_rr >= ccl_p1 && ccl_lr < ccl_p1) {
                                 // partially out of parking slot
                                 // TODO (p1, p2) x (lr, rr)
-                                // TODO (ccl, rr) x (p2, p3)
+                                auto cli2 = ::intersect(
+                                        cclx, ccly, ccl_rr,
+                                        this->ps().x2(), this->ps().y2(),
+                                        this->ps().x3(), this->ps().y3()
+                                );
+                                double a21 = ::angle_between_three_points(
+                                    this->cc().rrx(), this->cc().rry(),
+                                    cclx, ccly,
+                                    std::get<1>(cli2), std::get<2>(cli2)
+                                );
+                                double a22 = ::angle_between_three_points(
+                                    this->cc().rrx(), this->cc().rry(),
+                                    cclx, ccly,
+                                    std::get<3>(cli2), std::get<4>(cli2)
+                                );
+                                double a2 = std::min(a21, a22);
                         } else if (ccl_lr >= ccl_p1) {
                                 // in parking slot
                                 // TODO (ccl, lr) x (p1, p2)
-                                // TODO (ccl, rr) x (p2, p3)
+                                auto cli2 = ::intersect(
+                                        cclx, ccly, ccl_rr,
+                                        this->ps().x2(), this->ps().y2(),
+                                        this->ps().x3(), this->ps().y3()
+                                );
+                                double a21 = ::angle_between_three_points(
+                                    this->cc().rrx(), this->cc().rry(),
+                                    cclx, ccly,
+                                    std::get<1>(cli2), std::get<2>(cli2)
+                                );
+                                double a22 = ::angle_between_three_points(
+                                    this->cc().rrx(), this->cc().rry(),
+                                    cclx, ccly,
+                                    std::get<3>(cli2), std::get<4>(cli2)
+                                );
+                                double a2 = std::min(a21, a22);
                         }
 
                         double r1 = sqrt(