Set [-pi,pi] for bicycle car heading

The interval [-pi, pi] is due to compatibility with atan2 function.

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 571a2a6c4b34e9d6dd284c1794373520c658a5c2..3cc5fb73ce48191b9bcfed7d55ded192566d8a63 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.
 
+### Changed
+- When set up BicycleCar heading, set the interval to `[-pi, +pi]`.
+
 ### Fixed
 - Minimum turning radius used as curb-to-curb.
 

diff --git a/api/bcar.h b/api/bcar.h
index 716adab2b50022c1fcea1d4fc2321dab75abf466..4c83da17029a92688b3cad12ef89a0139de02b81 100644 (file)
--- a/api/bcar.h
+++ b/api/bcar.h
@@ -11,7 +11,7 @@ This class contains some geometrical computations of bicycle car.
 
 \param x Horizontal coordinate of rear axle center.
 \param y Vertical coordinate of rear axle center.
-\param h Heading of the car.
+\param h Heading of the car in the interval [-pi,+pi] radians.
 \param mtr Minimum turning radius.
 \param wb Wheelbase.
 \param w The width of the car.
@@ -131,7 +131,14 @@ class BicycleCar {
                 void y(double y) { this->y_ = y; }
 
                 double h() const { return this->h_; }
-                void h(double h) { this->h_ = h; }
+                void h(double h)
+                {
+                        while (h < -M_PI)
+                                h += 2 * M_PI;
+                        while (h > +M_PI)
+                                h -= 2 * M_PI;
+                        this->h_ = h;
+                }
 
                 double ctc() const { return this->ctc_; }
                 void ctc(double ctc) { this->ctc_ = ctc; }