Add reflection method

diff --git a/incl/bcar.hh b/incl/bcar.hh
index d62727d383825ca03070ad6962b75fffb9b6ab4a..095c2213ed73bc6198c0fc1f151693765bfc604a 100644 (file)
--- a/incl/bcar.hh
+++ b/incl/bcar.hh
@@ -65,6 +65,12 @@ public:
        */
        void rotate(Point const& c, double const angl);
 
+       /*! \brief Compute reflection of `this` around the `Line`.
+        *
+        * \param li The plane to reflect around is given by `li`.
+        */
+       void reflect(Line const& li);
+
        /*! Return Euclidean distance to `p`. */
        double edist(Point const& p) const;
 
@@ -141,6 +147,8 @@ public:
 
        void rotate(Point const& c, double const angl);
 
+       void reflect(Line const& li);
+
        friend std::ostream& operator<<(std::ostream& out, Pose const& p);
 };
 
@@ -163,6 +171,8 @@ public:
 
        void rotate(Point const& c, double const angl);
 
+       void reflect(Line const& li);
+
        friend std::ostream& operator<<(std::ostream& out, PoseRange const& p);
 };
 

diff --git a/src/bcar.cc b/src/bcar.cc
index d0d40a146b5abba0184d26d0cd87fcf1482bf233..0cdb078b43d14461c397fd8aefcb0752eabc14b4 100644 (file)
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -108,6 +108,16 @@ Point::rotate(Point const& c, double const angl)
        this->y(ny + c.y());
 }
 
+void
+Point::reflect(Line const& li)
+{
+       this->rotate(li.b(), -li.h());
+       this->y_ -= li.b().y();
+       this->y_ *= -1.0;
+       this->y_ += li.b().y();
+       this->rotate(li.b(), li.h());
+}
+
 double
 Point::edist(Point const& p) const
 {
@@ -272,6 +282,14 @@ Pose::rotate(Point const& c, double const angl)
        this->h(this->h() + angl);
 }
 
+void
+Pose::reflect(Line const& li)
+{
+       Point::reflect(li);
+       double dh = li.h() - this->h();
+       this->h(this->h() + 2.0 * dh);
+}
+
 std::ostream&
 operator<<(std::ostream& out, Pose const& p)
 {
@@ -316,6 +334,14 @@ PoseRange::rotate(Point const& c, double const angl)
        this->e(this->e() + angl);
 }
 
+void
+PoseRange::reflect(Line const& li)
+{
+       Pose::reflect(li);
+       double dh = li.h() - this->e();
+       this->e(this->e() + 2.0 * dh);
+}
+
 std::ostream&
 operator<<(std::ostream& out, PoseRange const& p)
 {