Add car frame point and side getters

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

diff --git a/src/bcar.cc b/src/bcar.cc
index 75f220c9d3668c1462eeb49af00e9c3dcf2ed7ed..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)
 {
@@ -79,6 +79,48 @@ Point::inside_of(std::vector<Point> const& poly) const
        return c;
 }
 
+bool
+Point::on_right_side_of(Line const& li) const
+{
+       auto x1 = li.fp().x();
+       auto y1 = li.fp().y();
+       auto x2 = li.lp().x();
+       auto y2 = li.lp().y();
+       auto x3 = this->x_;
+       auto y3 = this->y_;
+       if (sgn((x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1)) < 0.0) {
+               return false;
+       } else {
+               return true;
+       }
+}
+
+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)
+{
+       out << "[" << p.x() << "," << p.y() << "]";
+       return out;
+}
+
 Line::Line(Point const& fp, Point const& lp): first(fp), last(lp),
                intersection1(Point(0.0, 0.0)), intersection2(Point(0.0, 0.0))
 {
@@ -129,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));
@@ -175,20 +217,19 @@ Line::intersects_with(Point const& c, double const r)
        return true;
 }
 
-bool
-Line::is_on_right_side(Point const& p) const
+double
+Line::len() const
 {
-       auto x1 = this->fp().x();
-       auto y1 = this->fp().y();
-       auto x2 = this->lp().x();
-       auto y2 = this->lp().y();
-       auto x3 = p.x();
-       auto y3 = p.y();
-       if (sgn((x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1)) < 0.0) {
-               return false;
-       } else {
-               return true;
-       }
+       double dx = this->lp().x() - this->fp().x();
+       double dy = this->lp().y() - this->fp().y();
+       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()
@@ -228,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&
@@ -597,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
 {
@@ -653,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