From 03c136c4f372a34ee85ddc6334ecd79aec949b9d Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 19 Feb 2019 09:51:02 +0100
Subject: [PATCH] Add corner getters to bcar

---
 incl/bcar.h              |  12 +++++
 vehicle_platform/bcar.cc | 112 ++++++++++++++++++++++++++-------------
 2 files changed, 88 insertions(+), 36 deletions(-)

diff --git a/incl/bcar.h b/incl/bcar.h
index 4fffb93..387625a 100644
--- a/incl/bcar.h
+++ b/incl/bcar.h
@@ -39,6 +39,18 @@ class BicycleCar: public RRTNode {
                 using RRTNode::RRTNode;
 
                 // getter
+                float dr();
+                float df();
+
+                float lfx();
+                float lrx();
+                float rrx();
+                float rfx();
+                float lfy();
+                float lry();
+                float rry();
+                float rfy();
+
                 float speed();
                 float steer();
 
diff --git a/vehicle_platform/bcar.cc b/vehicle_platform/bcar.cc
index 9b36289..feeaaf8 100644
--- a/vehicle_platform/bcar.cc
+++ b/vehicle_platform/bcar.cc
@@ -18,86 +18,126 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #include <cmath>
 #include "bcar.h"
 
-float BicycleCar::speed()
+float BicycleCar::dr()
 {
-        return this->speed_;
+        return (this->length_ - this->wheel_base_) / 2;
 }
 
-float BicycleCar::steer()
+float BicycleCar::df()
 {
-        return this->steer_;
+        return this->length_ - this->dr();
 }
 
-bool BicycleCar::speed(float s)
+float BicycleCar::lfx()
 {
-        this->speed_ = s;
-        return true;
-}
-
-bool BicycleCar::steer(float s)
-{
-        this->steer_ = s;
-        return true;
-}
-
-std::vector<RRTEdge *> BicycleCar::frame()
-{
-        std::vector<RRTEdge *> frame;
-        float dr = (this->length_ - this->wheel_base_) / 2;
-        float df = this->length_ - dr;
 
 	float lfx = this->x();
         lfx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
-        lfx += df * cos(this->h());
+        lfx += this->df() * cos(this->h());
         lfx += this->safety_dist_ * cos(this->h());
+        return lfx;
+}
 
+float BicycleCar::lrx()
+{
         float lrx = this->x();
         lrx += (this->width_ / 2) * cos(this->h() + M_PI / 2);
-        lrx += -dr * cos(this->h());
+        lrx += -this->dr() * cos(this->h());
         lrx += -this->safety_dist_ * cos(this->h());
+        return lrx;
+}
 
+float BicycleCar::rrx()
+{
         float rrx = this->x();
         rrx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
-        rrx += -dr * cos(this->h());
+        rrx += -this->dr() * cos(this->h());
         rrx += -this->safety_dist_ * cos(this->h());
+        return rrx;
+}
 
+float BicycleCar::rfx()
+{
         float rfx = this->x();
         rfx += (this->width_ / 2) * cos(this->h() - M_PI / 2);
-        rfx += df * cos(this->h());
+        rfx += this->df() * cos(this->h());
         rfx += this->safety_dist_ * cos(this->h());
+        return rfx;
+}
 
+float BicycleCar::lfy()
+{
         float lfy = this->y();
         lfy += (this->width_ / 2) * sin(this->h() + M_PI / 2);
-        lfy += df * sin(this->h());
+        lfy += this->df() * sin(this->h());
         lfy += this->safety_dist_ * sin(this->h());
+        return lfy;
+}
 
+float BicycleCar::lry()
+{
         float lry = this->y();
         lry += (this->width_ / 2) * sin(this->h() + M_PI / 2);
-        lry += -dr * sin(this->h());
+        lry += -this->dr() * sin(this->h());
         lry += -this->safety_dist_ * sin(this->h());
+        return lry;
+}
 
+float BicycleCar::rry()
+{
         float rry = this->y();
         rry += (this->width_ / 2) * sin(this->h() - M_PI / 2);
-        rry += -dr * sin(this->h());
+        rry += -this->dr() * sin(this->h());
         rry += -this->safety_dist_ * sin(this->h());
+        return rry;
+}
 
+float BicycleCar::rfy()
+{
         float rfy = this->y();
         rfy += (this->width_ / 2) * sin(this->h() - M_PI / 2);
-        rfy += df * sin(this->h());
+        rfy += this->df() * sin(this->h());
         rfy += this->safety_dist_ * sin(this->h());
+        return rfy;
+}
 
+float BicycleCar::speed()
+{
+        return this->speed_;
+}
+
+float BicycleCar::steer()
+{
+        return this->steer_;
+}
+
+bool BicycleCar::speed(float s)
+{
+        this->speed_ = s;
+        return true;
+}
+
+bool BicycleCar::steer(float s)
+{
+        this->steer_ = s;
+        return true;
+}
+
+std::vector<RRTEdge *> BicycleCar::frame()
+{
+        std::vector<RRTEdge *> frame;
 	frame.push_back(new RRTEdge(
-                                new RRTNode(lfx, lfy),
-                                new RRTNode(lrx, lry)));
+                                new RRTNode(this->lfx(), this->lfy()),
+                                new RRTNode(this->lrx(), this->lry())));
 	frame.push_back(new RRTEdge(
-                                new RRTNode(lrx, lry),
-                                new RRTNode(rrx, rry)));
+                                new RRTNode(this->lrx(), this->lry()),
+                                new RRTNode(this->rrx(), this->rry())));
 	frame.push_back(new RRTEdge(
-                                new RRTNode(rrx, rry),
-                                new RRTNode(rfx, rfy)));
+                                new RRTNode(this->rrx(), this->rry()),
+                                new RRTNode(this->rfx(), this->rfy())));
 	frame.push_back(new RRTEdge(
-                                new RRTNode(rfx, rfy),
-                                new RRTNode(lfx, rfy)));
+                                new RRTNode(this->rfx(), this->rfy()),
+                                new RRTNode(this->lfx(), this->rfy())));
 	return frame;
 }
 
-- 
2.39.2