From 4d81f7da3215ca6b779c304c9793bfbd1d498a51 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 11 May 2020 15:13:04 +0200 Subject: [PATCH] Set [-pi,pi] for bicycle car heading The interval [-pi, pi] is due to compatibility with atan2 function. --- CHANGELOG.md | 3 +++ api/bcar.h | 11 +++++++++-- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 571a2a6..3cc5fb7 100644 --- 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 716adab..4c83da1 100644 --- 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; } -- 2.39.2