Check left front zone

diff --git a/src/bcar.cc b/src/bcar.cc
index c0eaf6bed3dbe818bae4a44af38b3e6b3a27f6e7..af96b510f99a1fef66827642e5734ad348031524 100644 (file)
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -9,7 +9,25 @@ bool BicycleCar::drivable(const BicycleCar &bc) const
                 a_1 += 2 * M_PI;
         while (a_1 > +M_PI)
                 a_1 -= 2 * M_PI;
+        double h_d = bc.h() - this->h();
+        while (h_d < -M_PI)
+                h_d += 2 * M_PI;
+        while (h_d > +M_PI)
+                h_d -= 2 * M_PI;
+        double a_2 = 0;
         if (0 <= a_1 && a_1 <= M_PI/2) { // left front
+                BicycleCar z(*this); // zone border
+                z.rotate(this->ccl().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 (z.h() >= a_2 && a_2 >= this->h())
+                        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
         } else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear