Merge branch 'feature/moving'

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 2cd510db15c2130460dc235e123889881d5fedca..ee5d92499f89315f611ebce65c090dee9cf810f8 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -14,5 +14,6 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Bicycle car frame coordinates.
 - [WvTest][] unit testing framework.
 - Minimum turning radius circle centers on left/right side.
+- Move internal parameters (`x`, `y`, `h`) based on speed `sp` and steer `st`.
 
 [WvTest]: https://github.com/apenwarr/wvtest

diff --git a/api/bcar.h b/api/bcar.h
index b401f1c7dce86c2cb45a37788d348ced967d9833..bd662bd27d5a11a4a1e5776d89fb37f49473925a 100644 (file)
--- a/api/bcar.h
+++ b/api/bcar.h
@@ -16,6 +16,8 @@ This class contains some geometrical computations of bicycle car.
 \param sd The safety distance.
 \param df Distance from rear axle center to the front of the car.
 \param dr Distance from rear axle center to the back of the car.
+\param sp Speed of the car.
+\param st Steering of the car.
 */
 class BicycleCar {
         private:
@@ -33,6 +35,9 @@ class BicycleCar {
                 double sd_ = 0;
                 double df_ = 3.105;
                 double dr_ = 0.655;
+                // moving
+                double sp_ = 0;
+                double st_ = 0;
         public:
                 // car frame
                 double lfx(); double lfy();
@@ -56,6 +61,13 @@ class BicycleCar {
                 */
                 BicycleCar *ccr();
 
+                // moving
+                /*! \brief Next car position based on `sp` and `st`.
+
+                Where `sp` is speed and `st` is steering of the car.
+                */
+                void next();
+
                 // getters, setters
                 double x() { return this->x_; }
                 void x(double x) { this->x_ = x; }
@@ -90,6 +102,12 @@ class BicycleCar {
                 double dr() { return this->dr_; }
                 void dr(double dr) { this->dr_ = dr; }
 
+                double sp() { return this->sp_; }
+                void sp(double sp) { this->sp_ = sp; }
+
+                double st() { return this->st_; }
+                void st(double st) { this->st_ = st; }
+
                 BicycleCar();
 };
 

diff --git a/src/bcar.cc b/src/bcar.cc
index a463822e27d24a8e03f87c52eb406c0031e0ff67..5e450f094f3782ea57a3c59ed5d01950a8be49b9 100644 (file)
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -119,6 +119,18 @@ BicycleCar *BicycleCar::ccr()
         return bc;
 }
 
+// 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());
+        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()));
+}
+
 BicycleCar::BicycleCar()
 {
 }

diff --git a/ut/bcar.t.cc b/ut/bcar.t.cc
index a4f4b857be20b07b73740640d43f0c0d42f8a452..f919e1b71cb2cac96b4de81c18a8da419d29bef3 100644 (file)
--- a/ut/bcar.t.cc
+++ b/ut/bcar.t.cc
@@ -38,4 +38,17 @@ WVTEST_MAIN("bcar basic geometry")
         WVPASSEQ_DOUBLE(M_PI / 2, bc->ccr()->h(), 0.00001);
         WVPASSEQ_DOUBLE(11, bc->ccr()->x(), 0.00001);
         WVPASSEQ_DOUBLE(1, bc->ccr()->y(), 0.00001);
+
+        bc->sp(1);
+        bc->st(0);
+        bc->next();
+        WVPASSEQ_DOUBLE(1, bc->x(), 0.00001);
+        WVPASSEQ_DOUBLE(2, bc->y(), 0.00001);
+
+        bc->st(M_PI);
+        bc->next();
+        WVPASSEQ_DOUBLE(0.2, bc->st(), 0.00001);
+        bc->st(-M_PI);
+        bc->next();
+        WVPASSEQ_DOUBLE(-0.2, bc->st(), 0.00001);
 }