From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 19 Jul 2021 11:58:58 +0000 (+0200)
Subject: Add reflection method
X-Git-Tag: v0.6.0~3^2~9
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/bcar.git/commitdiff_plain/1c821245b28276f370d4b786a236271356dd8c80?ds=sidebyside

Add reflection method
---

diff --git a/incl/bcar.hh b/incl/bcar.hh
index d62727d..095c221 100644
--- 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 d0d40a1..0cdb078 100644
--- 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)
 {