From b35f82dd3888d10edbf5054678ed3820de7d558b Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 11 May 2020 18:02:11 +0200 Subject: [PATCH] Check right front zone --- src/bcar.cc | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/bcar.cc b/src/bcar.cc index af96b51..b3665ac 100644 --- a/src/bcar.cc +++ b/src/bcar.cc @@ -30,6 +30,18 @@ bool BicycleCar::drivable(const BicycleCar &bc) const return true; } else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear } else if (0 > a_1 && a_1 >= -M_PI/2) { // right front + BicycleCar z(*this); // zone border + z.rotate(this->ccr().x(), this->ccl().y(), h_d); + // assert z.h() == bc.h() + if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border + return true; + a_2 = atan2(bc.y() - z.y(), bc.x() - z.x()); + while (a_2 < -M_PI) + a_2 += 2 * M_PI; + while (a_2 > +M_PI) + a_2 -= 2 * M_PI; + if (this->h() >= a_2 && a_2 >= z.h()) + return true; } else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear } else { // Not happenning, as ``-pi <= a <= pi``. -- 2.39.2