From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 7 Jul 2020 12:30:10 +0000 (+0200)
Subject: Add (ccr, rf) x (p3, p4) computation
X-Git-Tag: v0.4.0~4^2~9
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/commitdiff_plain/42b5948cacab452d4bdc6f465f6d48df57491184

Add (ccr, rf) x (p3, p4) computation
---

diff --git a/src/psp.cc b/src/psp.cc
index cff29ff..b9b26a5 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -523,7 +523,17 @@ void PSPlanner::fe_parallel()
                                         this->cc() = BicycleCar(tmp_cc);
                                 }
                         }
-                        // TODO (ccr, rf) x (p3, p4)
+                        auto cli1 = ::intersect(
+                                ccrx, ccry, ccr_rf,
+                                this->ps().x3(), this->ps().y3(),
+                                this->ps().x4(), this->ps().y4()
+                        );
+                        double a1 = smaller_angle_of_two(
+                                this->cc().rfx(), this->cc().rfy(),
+                                ccrx, ccry,
+                                std::get<1>(cli1), std::get<2>(cli1),
+                                std::get<3>(cli1), std::get<4>(cli1)
+                        );
                         // TODO (ccr, rf) x (p2, p3)
                         // TODO (ccr, lf) x (p3, p4)
 {