Add operator== for pose

[hubacji1/bcar.git] / src / bcar.cc

diff --git a/src/bcar.cc b/src/bcar.cc
index 8825fb77c63c1e088dcb19c78447962c2e82c624..93f017e7cacde2038ae0cf57e3092c163c659473 100644 (file)
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -108,12 +108,28 @@ 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
 {
        return sqrt(pow(p.x() - this->x_, 2.0) + pow(p.y() - this->y_, 2.0));
 }
 
+bool
+Point::operator==(Point const& p)
+{
+       return this->x() == p.x() && this->y() == p.y();
+}
+
 std::ostream&
 operator<<(std::ostream& out, Point const& p)
 {
@@ -222,6 +238,12 @@ Line::len() const
        return this->b_.edist(this->e_);
 }
 
+double
+Line::h() const
+{
+       return atan2(this->e_.y() - this->b_.y(), this->e_.x() - this->b_.x());
+}
+
 std::ostream&
 operator<<(std::ostream& out, Line const& li)
 {
@@ -266,6 +288,20 @@ 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);
+}
+
+bool
+Pose::operator==(Pose const& p)
+{
+       return this->x() == p.x() && this->y() == p.y() && this->h() == p.h();
+}
+
 std::ostream&
 operator<<(std::ostream& out, Pose const& p)
 {
@@ -310,6 +346,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)
 {