From 6a3e41c54aa847bf3af810e9f45a8e5f27c14478 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Tue, 7 Jul 2020 12:25:00 +0200 Subject: [PATCH] Add (ccl, lr) x (p1, p2) computation --- src/psp.cc | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/psp.cc b/src/psp.cc index 23f6e79..d6ce923 100644 --- 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(), -- 2.39.2