Add car frame point and side getters

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

diff --git a/src/bcar.cc b/src/bcar.cc
index 985537e61f90b250ae8e316b8961dbe0baca7102..32b2b8869ded7e6a1ec7ea3dcae717e1198ef43e 100644 (file)
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -1,7 +1,7 @@
 #include <cmath>
 #include "bcar.hh"
 
-using namespace bcar;
+namespace bcar {
 
 Point::Point(double x, double y) : x_(x), y_(y)
 {
@@ -95,6 +95,25 @@ Point::on_right_side_of(Line const& li) const
        }
 }
 
+void
+Point::rotate(Point const& c, double const angl)
+{
+       double px = this->x();
+       double py = this->y();
+       px -= c.x();
+       py -= c.y();
+       double nx = px * cos(angl) - py * sin(angl);
+       double ny = px * sin(angl) + py * cos(angl);
+       this->x(nx + c.x());
+       this->y(ny + c.y());
+}
+
+double
+Point::edist(Point const& p) const
+{
+       return sqrt(pow(p.x() - this->x_, 2.0) + pow(p.y() - this->y_, 2.0));
+}
+
 std::ostream&
 operator<<(std::ostream& out, Point const& p)
 {
@@ -152,7 +171,7 @@ Line::intersects_with(Line const& li)
        u *= -1.0;
        u /= deno;
        if (t < 0.0 || t > 1.0 || u < 0.0 || u > 1.0) {
-               false;
+               return false;
        }
        this->intersection1.x(x1 + t * (x2 - x1));
        this->intersection1.y(y1 + t * (y2 - y1));
@@ -206,6 +225,13 @@ Line::len() const
        return sqrt(dx * dx + dy * dy);
 }
 
+std::ostream&
+operator<<(std::ostream& out, Line const& li)
+{
+       out << "[" << li.first << "," << li.last << "]";
+       return out;
+}
+
 Pose::Pose() : Point()
 {
 }
@@ -243,15 +269,8 @@ Pose::set_pose(Pose const& p)
 void
 Pose::rotate(Point const& c, double const angl)
 {
-       double px = this->x();
-       double py = this->y();
-       px -= c.x();
-       py -= c.y();
-       double nx = px * cos(angl) - py * sin(angl);
-       double ny = px * sin(angl) + py * cos(angl);
+       Point::rotate(c, angl);
        this->h(this->h() + angl);
-       this->x(nx + c.x());
-       this->y(ny + c.y());
 }
 
 std::ostream&
@@ -612,6 +631,54 @@ BicycleCar::rfy() const
        return rfy;
 }
 
+Point
+BicycleCar::lf() const
+{
+       return Point(this->lfx(), this->lfy());
+}
+
+Point
+BicycleCar::lr() const
+{
+       return Point(this->lrx(), this->lry());
+}
+
+Point
+BicycleCar::rr() const
+{
+       return Point(this->rrx(), this->rry());
+}
+
+Point
+BicycleCar::rf() const
+{
+       return Point(this->rfx(), this->rfy());
+}
+
+Line
+BicycleCar::left() const
+{
+       return Line(this->lr(), this->lf());
+}
+
+Line
+BicycleCar::rear() const
+{
+       return Line(this->lr(), this->rr());
+}
+
+Line
+BicycleCar::right() const
+{
+       return Line(this->rr(), this->rf());
+}
+
+Line
+BicycleCar::front() const
+{
+       return Line(this->rf(), this->lf());
+}
+
 double
 BicycleCar::ralx() const
 {
@@ -668,3 +735,5 @@ BicycleCar::next()
        this->y(this->y() + this->sp() * sin(this->h()));
        this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
 }
+
+} // namespace bcar