Add turning radius circle centers of car

diff --git a/incl/bcar.h b/incl/bcar.h
index c5661e0521ea1a3920332b97d21b0ad8f6753fc0..64e1f1131bde612a87cafc4a2f63d721da5cbf50 100644 (file)
--- 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 74b7e4cdc1fba2c4749747a08a0b340c95e013e2..57ebee0d2da5c667b8e12d6c1e0dd0e32fe1bcc2 100644 (file)
--- 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()));
+}