Add pose range

diff --git a/incl/bcar.hh b/incl/bcar.hh
index 578e6f90764b31f752b939849c70ea969e202960..1ab5b58965e7d29cce30f956cbccf3a136243565 100644 (file)
--- 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 3c90942dc5c9b01b29311ef014f205e9dfff508f..638db3e03bc0a021e0105d840ca3cb9e1a8d7fcc 100644 (file)
--- 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
 {