From e9d4292c0b3d469342b684da477dec1ca4e68b70 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 19 Feb 2019 10:12:45 +0100
Subject: [PATCH] Add coordinates abreast with the rear axle

---
 incl/bcar.h              |  3 +++
 vehicle_platform/bcar.cc | 27 +++++++++++++++++++++++++++
 2 files changed, 30 insertions(+)

diff --git a/incl/bcar.h b/incl/bcar.h
index 387625a..c5661e0 100644
--- a/incl/bcar.h
+++ b/incl/bcar.h
@@ -51,6 +51,9 @@ class BicycleCar: public RRTNode {
                 float rry();
                 float rfy();
 
+                float lwb();
+                float rwb();
+
                 float speed();
                 float steer();
 
diff --git a/vehicle_platform/bcar.cc b/vehicle_platform/bcar.cc
index feeaaf8..74b7e4c 100644
--- a/vehicle_platform/bcar.cc
+++ b/vehicle_platform/bcar.cc
@@ -101,6 +101,33 @@ float BicycleCar::rfy()
         return rfy;
 }
 
+float BicycleCar::lwbx()
+{
+        float lrx = this->x();
+        lrx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
+        return lrx;
+}
+float BicycleCar::lwby()
+{
+        float lry = this->y();
+        lry += (this->width_ / 2) * sin(this->h() + M_PI / 2);
+        return lry;
+}
+
+float BicycleCar::rwbx()
+{
+        float rrx = this->x();
+        rrx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
+        return rrx;
+}
+
+float BicycleCar::rwby()
+{
+        float rry = this->y();
+        rry += (this->width_ / 2) * sin(this->h() - M_PI / 2);
+        return rry;
+}
+
 float BicycleCar::speed()
 {
         return this->speed_;
-- 
2.39.2