From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 7 Jul 2020 10:31:03 +0000 (+0200)
Subject: Add (p1, p2) x (lr, rr) computation
X-Git-Tag: v0.4.0~4^2~14
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/commitdiff_plain/691702ac3f1409db9e2147c49e71c5ae21b58848

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

diff --git a/src/psp.cc b/src/psp.cc
index d6ce923..66794b9 100644
--- 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(),