From 3fbaeea06a2f9d6fb2e44e3248f37c17f5641756 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 13 Jul 2021 14:10:38 +0200
Subject: [PATCH] Add pose range

---
 incl/bcar.hh | 22 ++++++++++++++++++++++
 src/bcar.cc  | 45 +++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 67 insertions(+)

diff --git a/incl/bcar.hh b/incl/bcar.hh
index 578e6f9..1ab5b58 100644
--- a/incl/bcar.hh
+++ b/incl/bcar.hh
@@ -137,6 +137,28 @@ public:
 	friend std::ostream& operator<<(std::ostream& out, Pose const& p);
 };
 
+class PoseRange : public Pose {
+private:
+	double e_ = 0.0;
+	using Pose::h;
+public:
+	/*! Get heading's begin in the interval [-pi, +pi] radians. */
+	double b() const;
+
+	/*! Set heading's begin in radians. It's recomputed to [-pi, +pi]. */
+	void b(double b);
+
+	/*! Get heading's end in the interval [-pi, +pi] radians. */
+	double e() const;
+
+	/*! Set heading's end in radians. It's recomputed to [-pi, +pi]. */
+	void e(double e);
+
+	void rotate(Point const& c, double const angl);
+
+	friend std::ostream& operator<<(std::ostream& out, PoseRange const& p);
+};
+
 /*! \brief Store car size.
  *
  * - Default is https://en.wikipedia.org/wiki/Fiat_Punto
diff --git a/src/bcar.cc b/src/bcar.cc
index 3c90942..638db3e 100644
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -254,6 +254,51 @@ operator<<(std::ostream& out, Pose const& p)
 	return out;
 }
 
+double
+PoseRange::b() const
+{
+	return this->h();
+}
+
+void
+PoseRange::b(double b)
+{
+	this->h(b);
+}
+
+double
+PoseRange::e() const
+{
+	return this->e_;
+}
+
+void
+PoseRange::e(double e)
+{
+	while (e < -M_PI) {
+		e += 2 * M_PI;
+	}
+	while (e > +M_PI) {
+		e -= 2 * M_PI;
+	}
+	this->e_ = e;
+}
+
+void
+PoseRange::rotate(Point const& c, double const angl)
+{
+	Pose::rotate(c, angl);
+	this->e(this->e() + angl);
+}
+
+std::ostream&
+operator<<(std::ostream& out, PoseRange const& p)
+{
+	out << "[" << p.x() << "," << p.y() << "," << p.b() << "," << p.e();
+	out << "]";
+	return out;
+}
+
 double
 CarSize::ctc() const
 {
-- 
2.39.2