Merge branch 'fix/mtr'

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 086f49d593f232fb36937280efcabd1af72ed107..571a2a6c4b34e9d6dd284c1794373520c658a5c2 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -11,6 +11,9 @@ The format is based on [Keep a Changelog][] and this project adheres to
 ### Added
 - Rotation of BicycleCar around the point.
 
+### Fixed
+- Minimum turning radius used as curb-to-curb.
+
 ## 0.3.0 - 2019-08-05
 ### Added
 - Reverse border method.

diff --git a/api/bcar.h b/api/bcar.h
index f698ecfc88c8705d579514d121f4bfe68a15cdc1..716adab2b50022c1fcea1d4fc2321dab75abf466 100644 (file)
--- a/api/bcar.h
+++ b/api/bcar.h
@@ -30,6 +30,8 @@ class BicycleCar {
                 double y_ = 0;
                 double h_ = 0;
                 // kinematic constraints
+                double ctc_ = 10.820; // curb-to-curb
+                // FIXME is not mtr; curb-to-curb is 10.820
                 double mtr_ = 10.820;
                 double wb_ = 2.450;
                 // dimensions
@@ -75,6 +77,15 @@ class BicycleCar {
                 the rear (from the rear axle view) part of the car.
                 */
                 double orradi() const;
+                /*! \brief Return length of perfect parking slot.
+
+                The width of the slot is the same as the width of the
+                car.
+                */
+                double perfect_parking_slot_len() const;
+                /*! \brief Set maximum steering angle.
+                */
+                void set_max_steer();
 
                 // car frame
                 double lfx() const; double lfy() const;
@@ -122,6 +133,9 @@ class BicycleCar {
                 double h() const { return this->h_; }
                 void h(double h) { this->h_ = h; }
 
+                double ctc() const { return this->ctc_; }
+                void ctc(double ctc) { this->ctc_ = ctc; }
+
                 double mtr() const { return this->mtr_; }
                 void mtr(double mtr) { this->mtr_ = mtr; }
 

diff --git a/src/bcar.cc b/src/bcar.cc
index c80d38bf131d8435319a8d5b3f92d2e12a80acaf..07d1e5d811f498358f1c1f64edb5f441d2fa4532 100644 (file)
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -34,6 +34,31 @@ double BicycleCar::orradi() const
         return sqrt(pow(this->mtr() + this->w() / 2, 2) + pow(this->dr(), 2));
 }
 
+double BicycleCar::perfect_parking_slot_len() const
+{
+        // see Simon R. Blackburn *The Geometry of Perfect Parking*
+        // see https://www.ma.rhul.ac.uk/SRBparking
+        double r = this->ctc() / 2;
+        double l = this->wb();
+        double k = this->df() - this->wb();
+        double w = this->w();
+        return
+                this->l()
+                + sqrt(
+                        (r*r - l*l)
+                        + pow(l + k, 2)
+                        - pow(sqrt(r*r - l*l) - w, 2)
+                )
+                - l
+                - k
+        ;
+}
+
+void BicycleCar::set_max_steer()
+{
+        this->st(atan(this->wb() / this->mtr()));
+}
+
 // car frame
 double BicycleCar::lfx() const
 {
@@ -155,10 +180,11 @@ BicycleCar BicycleCar::ccr() const
 // moving
 void BicycleCar::next()
 {
-        if (this->st() > this->wb() / this->mtr())
-                this->st(this->wb() / this->mtr());
-        if (this->st() < -this->wb() / this->mtr())
-                this->st(-this->wb() / this->mtr());
+        // FIXME
+        //if (this->st() > this->wb() / this->mtr())
+        //        this->st(this->wb() / this->mtr());
+        //if (this->st() < -this->wb() / this->mtr())
+        //        this->st(-this->wb() / this->mtr());
         this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
         this->x(this->x() + this->sp() * cos(this->h()));
         this->y(this->y() + this->sp() * sin(this->h()));
@@ -179,6 +205,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(