Implement method

diff --git a/src/bcar.cc b/src/bcar.cc
index a3203e0124e024e124ec3fa633c70a6b98cb53b3..ef4191858bce5387526d6595e5b7ba165c4de36a 100644 (file)
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -4,6 +4,18 @@
 // kinematic constraints
 bool BicycleCar::drivable(BicycleCar *bc)
 {
+        if (
+                pow(this->ccl()->x() - bc->x(), 2)
+                + pow(this->ccl()->y() - bc->y(), 2)
+                <= pow(this->mtr(), 2)
+        )
+                return false;
+        if (
+                pow(this->ccr()->x() - bc->x(), 2)
+                + pow(this->ccr()->y() - bc->y(), 2)
+                <= pow(this->mtr(), 2)
+        )
+                return false;
         return true;
 }