From ed0ae53e690bd38db258b5184981cbf8c3b05fc2 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Fri, 10 Jan 2020 15:41:10 +0100
Subject: [PATCH] Add mtr workaround

Define mtr as distance from the centre of rear axle to centre of U-turn.
---
 api/bcar.h  | 1 +
 src/bcar.cc | 7 +++++++
 2 files changed, 8 insertions(+)

diff --git a/api/bcar.h b/api/bcar.h
index f698ecf..bd36da7 100644
--- a/api/bcar.h
+++ b/api/bcar.h
@@ -30,6 +30,7 @@ class BicycleCar {
                 double y_ = 0;
                 double h_ = 0;
                 // kinematic constraints
+                // FIXME is not mtr; curb-to-curb is 10.820
                 double mtr_ = 10.820;
                 double wb_ = 2.450;
                 // dimensions
diff --git a/src/bcar.cc b/src/bcar.cc
index c80d38b..9f90476 100644
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -179,6 +179,13 @@ void BicycleCar::rotate(double cx, double cy, double angl)
 
 BicycleCar::BicycleCar()
 {
+        // TODO according to mtr_ FIXME
+        this->mtr_ = sqrt(
+                        pow(10.82 / 2, 2)
+                        - pow(this->wb(), 2)
+                )
+                - this->w() / 2
+        ;
 }
 
 std::tuple<bool, unsigned int, unsigned int> collide(
-- 
2.39.2