From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 19 Feb 2019 09:34:51 +0000 (+0100)
Subject: Add turning radius circle centers of car
X-Git-Tag: v0.5.0~2^2~2
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/d91fddf82cb6eb2142f3d6d37de322b0ebe580ff

Add turning radius circle centers of car
---

diff --git a/incl/bcar.h b/incl/bcar.h
index c5661e0..64e1f11 100644
--- a/incl/bcar.h
+++ b/incl/bcar.h
@@ -51,8 +51,15 @@ class BicycleCar: public RRTNode {
                 float rry();
                 float rfy();
 
-                float lwb();
-                float rwb();
+                float lwbx();
+                float lwby();
+                float rwbx();
+                float rwby();
+
+                /** Return circle center on the left side of the car */
+                RRTNode *ccl();
+                /** Return circle center on the right side of the car */
+                RRTNode *ccr();
 
                 float speed();
                 float steer();
@@ -65,6 +72,11 @@ class BicycleCar: public RRTNode {
                 std::vector<RRTEdge *> frame();
                 bool next();
                 bool next(float speed, float steer);
+
+                /** Outer radius of the farthest point */
+                float out_radi();
+                /** Angle between wheelbase line and outer radius */
+                float alfa();
 };
 
 #endif
diff --git a/vehicle_platform/bcar.cc b/vehicle_platform/bcar.cc
index 74b7e4c..57ebee0 100644
--- a/vehicle_platform/bcar.cc
+++ b/vehicle_platform/bcar.cc
@@ -128,6 +128,34 @@ float BicycleCar::rwby()
         return rry;
 }
 
+RRTNode *BicycleCar::ccl()
+{
+        RRTEdge *l1 = new RRTEdge(
+                        new RRTNode(this->lwbx(), this->lwby()),
+                        new RRTNode(this->rwbx(), this->rwby()));
+        float d = this->width_ * tan(this->alfa());
+        float x = this->lfx() + d * cos(this->h() + M_PI);
+        float y = this->lfy() + d * sin(this->h() + M_PI);
+        RRTEdge *l2 = new RRTEdge(
+                        new RRTNode(x, y, 0),
+                        new RRTNode(this->rfx(), this->rfy(), 0));
+        return l1->intersect(l2, false);
+}
+
+RRTNode *BicycleCar::ccr()
+{
+        RRTEdge *l1 = new RRTEdge(
+                        new RRTNode(this->lwbx(), this->lwby()),
+                        new RRTNode(this->rwbx(), this->rwby()));
+        float d = this->width_ * tan(this->alfa());
+        float x = this->rfx() + d * cos(this->h() - M_PI);
+        float y = this->rfy() + d * sin(this->h() - M_PI);
+        RRTEdge *l2 = new RRTEdge(
+                        new RRTNode(this->lfx(), this->lfy(), 0),
+                        new RRTNode(x, y, 0));
+        return l1->intersect(l2, false);
+}
+
 float BicycleCar::speed()
 {
         return this->speed_;
@@ -190,3 +218,15 @@ bool BicycleCar::next(float speed, float steer)
         this->steer_ = steer;
         return this->next();
 }
+
+float BicycleCar::out_radi()
+{
+        return pow((pow(this->turning_radius_ + this->width_ / 2, 2) +
+                        pow((this->length_ + this->wheel_base_) / 2, 2)), 0.5);
+}
+
+float BicycleCar::alfa()
+{
+        return asin((this->length_ + this->wheel_base_) /
+                        (2 * this->out_radi()));
+}