#include <cmath>
#include "bcar.hh"
-using namespace bcar;
+namespace bcar {
Point::Point(double x, double y) : x_(x), y_(y)
{
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))
{
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));
return true;
}
-bool
-Line::is_on_right_side(Point const& p) 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
-Pose::x() const
+Line::len() const
{
- return this->x_;
+ double dx = this->lp().x() - this->fp().x();
+ double dy = this->lp().y() - this->fp().y();
+ return sqrt(dx * dx + dy * dy);
}
-void
-Pose::x(double x)
+std::ostream&
+operator<<(std::ostream& out, Line const& li)
{
- this->x_ = x;
+ out << "[" << li.first << "," << li.last << "]";
+ return out;
}
-double
-Pose::y() const
+Pose::Pose() : Point()
{
- return this->y_;
}
-void
-Pose::y(double y)
+Pose::Pose(double x, double y, double h) : Point(x, y), h_(h)
{
- this->y_ = y;
}
double
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&
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
{
this->y(this->y() + this->sp() * sin(this->h()));
this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
}
+
+} // namespace bcar