Check right front zone

diff --git a/src/bcar.cc b/src/bcar.cc
index af96b510f99a1fef66827642e5734ad348031524..b3665ac15e58bc17091a06dbe30487c910bbb19b 100644 (file)
--- 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``.