Add switch for bc direction

The ``a_1`` should be direction of ``bc`` related to ``this``.

diff --git a/src/bcar.cc b/src/bcar.cc
index 2c302b7cf7c9532ae29d8373c7a243dbef28899a..c0eaf6bed3dbe818bae4a44af38b3e6b3a27f6e7 100644 (file)
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -4,6 +4,18 @@
 // kinematic constraints
 bool BicycleCar::drivable(const BicycleCar &bc) const
 {
+        double a_1 = atan2(bc.y() - this->y(), bc.x() - this->x()) - this->h();
+        while (a_1 < -M_PI)
+                a_1 += 2 * M_PI;
+        while (a_1 > +M_PI)
+                a_1 -= 2 * M_PI;
+        if (0 <= a_1 && a_1 <= M_PI/2) { // left front
+        } 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
+        } else {
+                // Not happenning, as ``-pi <= a <= pi``.
+        }
         return false;
 }