Add (ccr, lf) x (p3, p4) computation

[hubacji1/psp.git] / src / psp.cc

diff --git a/src/psp.cc b/src/psp.cc
index cff29ffcbe75b09c193a835091859e83ae33871a..99f5d3d71d1c1f0e460ce6fd287e291891cfee41 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -523,9 +523,39 @@ void PSPlanner::fe_parallel()
                                         this->cc() = BicycleCar(tmp_cc);
                                 }
                         }
-                        // TODO (ccr, rf) x (p3, p4)
-                        // TODO (ccr, rf) x (p2, p3)
-                        // TODO (ccr, lf) 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)
+                        );
+                        auto cli2 = ::intersect(
+                                ccrx, ccry, ccr_rf,
+                                this->ps().x2(), this->ps().y2(),
+                                this->ps().x3(), this->ps().y3()
+                        );
+                        double a2 = smaller_angle_of_two(
+                                this->cc().rfx(), this->cc().rfy(),
+                                ccrx, ccry,
+                                std::get<1>(cli2), std::get<2>(cli2),
+                                std::get<3>(cli2), std::get<4>(cli2)
+                        );
+                        auto cli3 = ::intersect(
+                                ccrx, ccry, ccr_lf,
+                                this->ps().x3(), this->ps().y3(),
+                                this->ps().x4(), this->ps().y4()
+                        );
+                        double a3 = smaller_angle_of_two(
+                                this->cc().lfx(), this->cc().lfy(),
+                                ccrx, ccry,
+                                std::get<1>(cli3), std::get<2>(cli3),
+                                std::get<3>(cli3), std::get<4>(cli3)
+                        );
 {
         double rf = sqrt(
                 pow(this->cc().lfy() - ccry, 2)