From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 11 May 2020 14:41:36 +0000 (+0200)
Subject: Add switch for bc direction
X-Git-Tag: v0.4.0~12^2~5
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/bcar.git/commitdiff_plain/2ddef9682c251285f7a50e4cdb8889df131d0cd4

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 2c302b7..c0eaf6b 100644
--- 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;
 }