From 8c1187d32c5e8939276a62b575977980e212a976 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Tue, 13 Jul 2021 09:38:32 +0200 Subject: [PATCH] Refactor bcar --- incl/bcar.hh | 568 ++++++++++++++++++++++--------------------- src/bcar.cc | 665 +++++++++++++++++++++++++++++++++------------------ 2 files changed, 724 insertions(+), 509 deletions(-) diff --git a/incl/bcar.hh b/incl/bcar.hh index cecfe7e..bfb9cda 100644 --- a/incl/bcar.hh +++ b/incl/bcar.hh @@ -1,308 +1,320 @@ /*! \file */ -#ifndef BCAR_H -#define BCAR_H +#ifndef BCAR_BCAR_H +#define BCAR_BCAR_H -#include #include -#include #include -/*! \brief Bicycle car basic class. - -This class contains some geometrical computations of bicycle car. - -\param x Horizontal coordinate of rear axle center. -\param y Vertical coordinate of rear axle center. -\param h Heading of the car in the interval [-pi,+pi] radians. -\param mtr Minimum turning radius. -\param wb Wheelbase. -\param w The width of the car. -\param l The length of the car. -\param he The height of the car. -\param sd The safety distance. -\param df Distance from rear axle center to the front of the car. -\param dr Distance from rear axle center to the back of the car. -\param sp Speed of the car. -\param st Steering of the car. -*/ -class BicycleCar { +namespace bcar { + +template int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + +class Point { private: - // coordinates - double x_ = 0; - double y_ = 0; - double h_ = 0; - // kinematic constraints - double ctc_ = 10.820; // curb-to-curb - // FIXME is not mtr; curb-to-curb is 10.820 - double mtr_ = 10.820; - double wb_ = 2.450; - // dimensions - double w_ = 1.625; - double l_ = 3.760; - double he_ = 1.450; - double sd_ = 0; - double df_ = 3.105; - double dr_ = 0.655; - // moving - double sp_ = 0; - double st_ = 0; + double x_ = 0.0; + double y_ = 0.0; public: - // kinematic constraints - /*! \brief Return `false` if `bc` is not achievable. + Point(double x, double y); + Point(); + + /*! Get horizontal coordinate. */ + double x() const; + + /*! Set horizontal coordinate. */ + void x(double x); - When `false` is returned the `bc` may still be drivable, - because only "line segment - circle arc - line segment" - paths are considered in ``drivable`` method. + /*! Get vertical coordinate. */ + double y() const; - \param[in] bc The bicycle car to achieve. + /*! Set vertical coordinate. */ + void y(double y); + + /*! \brief Return the smallest angle between three points. + + \see https://math.stackexchange.com/questions/361412/finding-the-angle-between-three-points */ - bool drivable(const BicycleCar &bc) const; - bool drivable(const BicycleCar &bc, double b, double e) const; - /*! \brief Return inner radius. + double min_angle_between(Point const& p1, Point const& p2) const; + + /*! \brief Return `true` if `this` point is inside of polygon `poly`. + * + * The polygon is given by the vector of `Point`s. + * + * \see https://en.wikipedia.org/wiki/Even%E2%80%93odd_rule + * + * \param poly Polygon to consider. + */ + bool inside_of(std::vector const& poly) const; +}; + +class Line { +private: + Point first; + Point last; + Point intersection1; + Point intersection2; +public: + Line(Point const& fp, Point const& lp); + + /*! Get first point. */ + Point fp() const&; + + /*! Get last point. */ + Point lp() const&; + + /*! Get intersection point. */ + Point in1() const&; + + /*! Get intersection point. */ + Point in2() const&; + + /*! \brief Return if `this` line intersects with line `li`. + * + * If the method returns `true`, the intersection `Point` is available + * in `this->in1()`. + * + * \see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection + * + * \param li The line to check the intersection with. + */ + bool intersects_with(Line const& li); + + /*! \brief Return intersections of `this` (infinite) line and circle. + * + * If the method returns `true`, the intersection `Point`s are available + * in `this->in1()` and `this->in2()`. + * + * \see https://mathworld.wolfram.com/Circle-LineIntersection.html + * + * \param c Circle center. + * \param r Circle radius. + */ + bool intersects_with(Point const& c, double const r); + + /*! \brief Return if point `p` is on the right side of the plane. + * + * The plane is given by the line `this`, where `this->fp()` is the base + * point and the direction is given by `this->lp() - this->fp()`. + * + * \param p The point to consider. + */ + bool is_on_right_side(Point const& p) const; +}; + +/*! Store coordinates `x`, `y`, and heading `h`. */ +class Pose { +private: + double x_ = 0.0; + double y_ = 0.0; + double h_ = 0.0; +public: + /*! Get horizontal coordinate. */ + double x() const; + + /*! Set horizontal coordinate. */ + void x(double x); + + /*! Get vertical coordinate. */ + double y() const; + + /*! Set vertical coordinate. */ + void y(double y); + + /*! Get heading in the interval [-pi, +pi] radians. */ + double h() const; - The inner radius is the distance from minimum turning - radius circle center to the nearest point on the car. In - this case, the nearest points on the car are rear axle - endpoints. + /*! Set heading in radians. It's recomputed to [-pi, +pi]. */ + void h(double h); + + /*! \brief Rotate self around the point. + + \param c Rotation center `Point`. + \param angl Angle of rotation. */ + void rotate(Point const& c, double const angl); + + friend std::ostream& operator<<(std::ostream& out, Pose const& p); +}; + +/*! \brief Store car size. + * + * - Default is https://en.wikipedia.org/wiki/Fiat_Punto + */ +class CarSize { +private: + double curb_to_curb = 10.820; + double width = 1.625; + double wheelbase = 2.450; + double distance_to_front = 3.105; + double length = 3.760; +public: + /*! Get curb-to-curb distance. */ + double ctc() const; + + /*! Set curb-to-curb distance. */ + void ctc(double ctc); + + /*! Get wheelbase. */ + double wb() const; + + /*! Set wheelbase. */ + void wb(double wb); + + /*! Get width. */ + double w() const; + + /*! Set width. */ + void w(double w); + + /*! Get length. */ + double len() const; + + /*! Set length. */ + void len(double len); + + /*! Get distance from rear axle to front. */ + double df() const; + + /*! Set distance from rear axle to front. */ + void df(double df); + + /*! Get distance from rear axle to rear. */ + double dr() const; + + /*! \brief Get minimum turning radius. + * + * Please, note that the method returns really _minimum turning radius_, + * which is the distance from the reare axle center to the center of + * left or right rotation given by the kinematics constrants, i.e. + * _wheelbase_ and _curb-to-curb_ distance. + * + * Sometimes _minimum turning radius_ is not radius, not minimum, or not + * turning. In this method, _minimum turning radius_ is minimum turning + * radius. + */ + double mtr() const; +}; + +/*! Store car motion. */ +class CarMove { +private: + double speed = 0.0; + double steer = 0.0; +public: + /*! Get speed. */ + double sp() const; + + /*! Set speed. */ + void sp(double sp); + + /*! Get steer. */ + double st() const; + + /*! Set steer. */ + void st(double st); +}; + +/*! \brief Geometrical computations of a bicycle car. + * + * - `x()` and `y()` methods returns coordinates of rear axle center. + */ +class BicycleCar : public Pose, public CarSize, public CarMove { +private: +public: + /*! \brief Return `false` if `bc` is not achievable. + * + * When `false` is returned the `bc` may still be drivable, but not + * trivially, i.e. by "line segment - circle arc - line segment". + * + * \param bc The bicycle car to achieve. + * \param b The beginning of the heading range. + * \param e The end of the heading range. + */ + bool drivable(BicycleCar const& bc, double b, double e) const; + bool drivable(BicycleCar const& bc) const; + + /*! \brief Return inner radius. + * + * The inner radius is the distance from minimum turning radius circle + * center to the nearest point on the car. In this case, the nearest + * points on the car are rear axle endpoints. + */ double iradi() const; - /*! \brief Return outer front radius. - The outer front radius is the distance from minimum - turning radius circle center to the farthest point on - the front (from the rear axle view) part of the car. - */ + /*! \brief Return outer front radius. + * + * The outer front radius is the distance from minimum turning radius + * circle center to the farthest point on the front (from the rear axle + * view) part of the car. + */ double ofradi() const; - /*! \brief Return outer rear radius. - The outer rear radius is the distance from minimum - turning radius circle center to the farthest point on - the rear (from the rear axle view) part of the car. - */ + /*! \brief Return outer rear radius. + * + * The outer rear radius is the distance from minimum turning radius + * circle center to the farthest point on the rear (from the rear axle + * view) part of the car. + */ double orradi() const; - /*! \brief Return length of perfect parking slot. - The width of the slot is the same as the width of the - car. - */ + /*! \brief Return length of perfect parking slot. + * + * The width of the slot is the same as the width of the car. + * + * \see Simon R. Blackburn *The Geometry of Perfect Parking* + * \see https://www.ma.rhul.ac.uk/SRBparking + */ double perfect_parking_slot_len() const; - /*! \brief Set maximum steering angle. - */ + + /*! Set maximum steering angle. */ void set_max_steer(); - // car frame - double lfx() const; double lfy() const; - double lrx() const; double lry() const; - double rrx() const; double rry() const; - double rfx() const; double rfy() const; + /*! Get frame's left front x coordinate. */ + double lfx() const; - double ralx() const; double raly() const; - double rarx() const; double rary() const; + /*! Get frame's left front y coordinate. */ + double lfy() const; - /*! \brief Min. turning radius circle center on left. + /*! Get frame's left rear x coordinate. */ + double lrx() const; - Important are coordinates `x` and `y`. The heading `h` - is set as the heading of `this->h()`. - */ - BicycleCar ccl() const; - /*! \brief Min. turning radius circle center on rigth. + /*! Get frame's left rear y coordinate. */ + double lry() const; - Important are coordinates `x` and `y`. The heading `h` - is set as the heading of `this->h()`. - */ - BicycleCar ccr() const; + /*! Get frame's right rear x coordinate. */ + double rrx() const; - // moving - /*! \brief Next car position based on `sp` and `st`. + /*! Get frame's right rear y coordinate. */ + double rry() const; - Where `sp` is speed and `st` is steering of the car. - */ - void next(); - /*! \brief Rotate self around the point. + /*! Get frame's right front x coordinate. */ + double rfx() const; - \param cx Horizontal coordinate of rotation center. - \param cy Vertical coordinate of rotation center. - \param angl Angle of rotation. - */ - void rotate(double cx, double cy, double angl); - - // getters, setters - double x() const { return this->x_; } - void x(double x) { this->x_ = x; } - - double y() const { return this->y_; } - void y(double y) { this->y_ = y; } - - double h() const { return this->h_; } - void h(double h) - { - while (h < -M_PI) - h += 2 * M_PI; - while (h > +M_PI) - h -= 2 * M_PI; - this->h_ = h; - } - - double ctc() const { return this->ctc_; } - void ctc(double ctc) { this->ctc_ = ctc; } - - double mtr() const { return this->mtr_; } - void mtr(double mtr) { this->mtr_ = mtr; } - - double wb() const { return this->wb_; } - void wb(double wb) { this->wb_ = wb; } - - double w() const { return this->w_; } - void w(double w) { this->w_ = w; } - - double l() const { return this->l_; } - void l(double l) { this->l_ = l; } - - double he() const { return this->he_; } - void he(double he) { this->he_ = he; } - - double sd() const { return this->sd_; } - void sd(double sd) { this->sd_ = sd; } - - double df() const { return this->df_; } - void df(double df) { this->df_ = df; } - - double dr() const { return this->dr_; } - void dr(double dr) { this->dr_ = dr; } - - double sp() const { return this->sp_; } - void sp(double sp) { this->sp_ = sp; } - - double st() const { return this->st_; } - void st(double st) { this->st_ = st; } - - BicycleCar(); - friend std::ostream &operator<<( - std::ostream &out, - const BicycleCar &bc - ) - { - out << "[" << bc.x(); - out << "," << bc.y(); - out << "," << bc.h(); - out << "]"; - return out; - } + /*! Get frame's right front y coordinate. */ + double rfy() const; + + /*! Get rear axle's left x coordinate. */ + double ralx() const; + + /*! Get rear axle's left y coordinate. */ + double raly() const; + + /*! Get rear axle's right x coordinate. */ + double rarx() const; + + /*! Get rear axle's right y coordinate. */ + double rary() const; + + /*! Min. turning radius circle center on left. */ + Point ccl() const; + + /*! Min. turning radius circle center on rigth. */ + Point ccr() const; + + /*! Next car position based on speed `sp` and steer `st`. */ + void next(); }; -/*! \brief Does two polygons collide? - -Return the tuple `std::tuple`, where the first value is -`true` when there is an intersection of some segments of the polygons -`p1` and `p2` and `false` otherwise. The second and third parameters in -the return tuple are indexes of the first collision, where index starts -at 0. - -\param p1 The first polygon to check against collision. -\param p2 The second polygon to check against collision. -*/ -std::tuple -collide( - std::vector> &p1, - std::vector> &p2 -); - -/*! \brief Is `x, y` coordinate in polygon `poly`? - -Return `true` if `x, y` coordinate is inside of polygon `poly`. - -\see https://en.wikipedia.org/wiki/Even%E2%80%93odd_rule - -\param x Horizontal coordinate. -\param y Vertical coordinate. -\param poly The vector of coordinates. -*/ -bool -inside(double x, double y, std::vector> &poly); - -/*! \brief Return intersection of two line segments. - -The output is tuple `std::tuple`, where the first -value is true when there is an intersection and false otherwise. The -second and third parameters in the return tuple are coordinates of the -intersection. - -\see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection - -\param x1 First line segment first `x` coordinate. -\param y1 First line segment first `y` coordinate. -\param x2 First line segment second `x` coordinate. -\param y2 First line segment second `y` coordinate. -\param x3 Second line segment first `x` coordinate. -\param y3 Second line segment first `y` coordinate. -\param x4 Second line segment second `x` coordinate. -\param y4 Second line segment second `y` coordinate. -*/ -std::tuple -intersect( - double x1, double y1, - double x2, double y2, - double x3, double y3, - double x4, double y4 -); - -/*! \brief Return intersections of (infinite) line and circle. - -The output is tuple `std::tuble`, where -the first value is true when there is an intersection and false otherwise. The -second and third parameters in the return tuple are coordinates of the first -intersection. The fourth and fifth parameters in the return tuple are -coordinates of the second intersection. - -\see https://mathworld.wolfram.com/Circle-LineIntersection.html - -\param cx Circle center `x` coordinate. -\param cy Circle center `y` coordinate. -\param r Circle radius. -\param x1 Line segment first `x` coordinate. -\param y1 Line segment first `y` coordinate. -\param x2 Line segment second `x` coordinate. -\param y2 Line segment second `y` coordinate. -*/ -std::tuple -intersect( - double cx, double cy, double r, - double x1, double y1, - double x2, double y2 -); - -/*! \brief Return the smallest angle between three points. - -\see https://math.stackexchange.com/questions/361412/finding-the-angle-between-three-points - -\param x1 -\param y1 -\param x2 -\param y2 -\param x3 -\param y3 -*/ -double -angle_between_three_points( - double x1, double y1, - double x2, double y2, - double x3, double y3 -); - -/*! \brief Return if point is on the right side of plane. - -\param x1 Line first `x` coordinate. -\param y1 Line first `y` coordinate. -\param x2 Line second `x` coordinate. -\param y2 Line second `y` coordinate. -\param x3 Point to decide `x` coordinate. -\param y3 Point to decide `y` coordinate. -*/ -bool -right_side_of_line( - double x1, double y1, - double x2, double y2, - double x3, double y3 -); - -#endif /* BCAR_H */ +} // namespace bcar +#endif /* BCAR_BCAR_H */ diff --git a/src/bcar.cc b/src/bcar.cc index 2b4f92e..4305b7c 100644 --- a/src/bcar.cc +++ b/src/bcar.cc @@ -1,12 +1,365 @@ +#include #include "bcar.hh" -#include "pslot.hh" -// kinematic constraints -bool BicycleCar::drivable(const BicycleCar &bc) const +using namespace bcar; + +Point::Point(double x, double y) : x_(x), y_(y) +{ +} + +Point::Point() : Point::Point(0.0, 0.0) +{ +} + +double +Point::x() const +{ + return this->x_; +} + +void +Point::x(double x) +{ + this->x_ = x; +} + +double +Point::y() const +{ + return this->y_; +} + +void +Point::y(double y) +{ + this->y_ = y; +} + +double +Point::min_angle_between(Point const& p1, Point const& p2) const +{ + double d1x = p1.x() - this->x(); + double d1y = p1.y() - this->y(); + double d2x = p2.x() - p1.x(); + double d2y = p2.y() - p1.y(); + + double dot = d1x*d2x + d1y*d2y; + double d1 = sqrt(d1x*d1x + d1y*d1y); + double d2 = sqrt(d2x*d2x + d2y*d2y); + + double delta = acos(dot / (d1 * d2)); + return std::min(delta, M_PI - delta); +} + +bool +Point::inside_of(std::vector const& poly) const +{ + unsigned int num = poly.size(); + unsigned int j = num - 1; + bool c = false; + for (unsigned int i = 0; i < num; i++) { + if (this->x() == poly[i].x() && this->y() == poly[i].y()) { + return true; + } + if ((poly[i].y() > this->y()) != (poly[j].y() > this->y())) { + auto slope1 = this->x() - poly[i].x(); + slope1 *= poly[j].y() - poly[i].y(); + auto slope2 = poly[j].x() - poly[i].x(); + slope2 *= this->y() - poly[i].y(); + auto slope = slope1 - slope2; + if (slope == 0.0) { + return true; + } + if ((slope < 0.0) != (poly[j].y() < poly[i].y())) { + c = !c; + } + } + j = i; + } + return c; +} + +Line::Line(Point const& fp, Point const& lp): first(fp), last(lp), + intersection1(Point(0.0, 0.0)), intersection2(Point(0.0, 0.0)) +{ +} + +Point +Line::fp() const& +{ + return this->first; +} + +Point +Line::lp() const& +{ + return this->last; +} + +Point +Line::in1() const& +{ + return this->intersection1; +} + +Point +Line::in2() const& +{ + return this->intersection2; +} + +bool +Line::intersects_with(Line const& li) +{ + auto x1 = this->fp().x(); + auto y1 = this->fp().y(); + auto x2 = this->lp().x(); + auto y2 = this->lp().y(); + auto x3 = li.fp().x(); + auto y3 = li.fp().y(); + auto x4 = li.lp().x(); + auto y4 = li.lp().y(); + double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4); + if (deno == 0.0) { + return false; + } + double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4); + t /= deno; + double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3); + u *= -1.0; + u /= deno; + if (t < 0.0 || t > 1.0 || u < 0.0 || u > 1.0) { + false; + } + this->intersection1.x(x1 + t * (x2 - x1)); + this->intersection1.y(y1 + t * (y2 - y1)); + return true; +} + +bool +Line::intersects_with(Point const& c, double const r) +{ + auto x1 = this->fp().x(); + auto y1 = this->fp().y(); + auto x2 = this->lp().x(); + auto y2 = this->lp().y(); + auto cx = c.x(); + auto cy = c.y(); + x2 -= cx; + x1 -= cx; + y2 -= cy; + y1 -= cy; + if (y1 == y2) { + y1 += 0.00001; + } + double dx = x2 - x1; + double dy = y2 - y1; + double dr = sqrt(dx*dx + dy*dy); + double D = x1*y2 - x2*y1; + if (r*r * dr*dr - D*D < 0.0) { + return false; + } + // intersection coordinates + double ix1 = (D*dy + sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr); + ix1 += cx; + double ix2 = (D*dy - sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr); + ix2 += cx; + double iy1 = (-D*dx + std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr); + iy1 += cy; + double iy2 = (-D*dx - std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr); + iy2 += cy; + this->intersection1.x(ix1); + this->intersection1.y(iy1); + this->intersection2.x(ix2); + this->intersection2.y(iy2); + 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 +{ + return this->x_; +} + +void +Pose::x(double x) +{ + this->x_ = x; +} + +double +Pose::y() const +{ + return this->y_; +} + +void +Pose::y(double y) +{ + this->y_ = y; +} + +double +Pose::h() const +{ + return this->h_; +} + +void +Pose::h(double h) +{ + while (h < -M_PI) { + h += 2 * M_PI; + } + while (h > +M_PI) { + h -= 2 * M_PI; + } + this->h_ = h; +} + +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); + this->h(this->h() + angl); + this->x(nx + c.x()); + this->y(ny + c.y()); +} + +std::ostream& +operator<<(std::ostream& out, Pose const& p) +{ + out << "[" << p.x() << "," << p.y() << "," << p.h() << "]"; + return out; +} + +double +CarSize::ctc() const +{ + return this->curb_to_curb; +} + +void +CarSize::ctc(double ctc) +{ + this->curb_to_curb = ctc; +} + +double +CarSize::wb() const +{ + return this->wheelbase; +} + +void +CarSize::wb(double wb) +{ + this->wheelbase = wb; +} + +double +CarSize::w() const +{ + return this->width; +} + +void +CarSize::w(double w) +{ + this->width = w; +} + +double +CarSize::len() const +{ + return this->length; +} + +void +CarSize::len(double len) +{ + this->length = len; +} + +double +CarSize::df() const +{ + return this->distance_to_front; +} + +void +CarSize::df(double df) +{ + this->distance_to_front = df; +} + +double +CarSize::dr() const +{ + return this->len() - this->df(); +} + +double +CarSize::mtr() const +{ + auto ctc2 = pow(this->ctc() / 2.0, 2.0); + auto wb2 = pow(this->wb(), 2.0); + return sqrt(ctc2 - wb2) - this->w() / 2.0; +} + +double +CarMove::sp() const +{ + return this->speed; +} + +void +CarMove::sp(double sp) +{ + this->speed = sp; +} + +double +CarMove::st() const +{ + return this->steer; +} + +void +CarMove::st(double st) +{ + this->steer = st; +} + +bool +BicycleCar::drivable(BicycleCar const& bc) const { return this->drivable(bc, bc.h(), bc.h()); } -bool BicycleCar::drivable(const BicycleCar &bc, double b, double e) const + +bool +BicycleCar::drivable(BicycleCar const& bc, double b, double e) const { // assert bc.h() == (b + e) / 2.0 double a_1 = atan2(bc.y() - this->y(), bc.x() - this->x()) - this->h(); @@ -26,7 +379,7 @@ bool BicycleCar::drivable(const BicycleCar &bc, double b, double e) const BicycleCar z(*this); // zone border z.h(e); h_d = bc.h() - this->h(); - z.rotate(this->ccl().x(), this->ccl().y(), h_d); + z.rotate(this->ccl(), h_d); // assert z.h() == bc.h() if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border return true; @@ -41,7 +394,7 @@ bool BicycleCar::drivable(const BicycleCar &bc, double b, double e) const BicycleCar z(*this); // zone border z.h(e); h_d = bc.h() - this->h(); - z.rotate(this->ccl().x(), this->ccl().y(), h_d); + z.rotate(this->ccl(), h_d); // assert z.h() == bc.h() if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border return true; @@ -57,7 +410,7 @@ bool BicycleCar::drivable(const BicycleCar &bc, double b, double e) const BicycleCar z(*this); // zone border z.h(b); h_d = bc.h() - this->h(); - z.rotate(this->ccr().x(), this->ccr().y(), h_d); + z.rotate(this->ccr(), h_d); // assert z.h() == bc.h() if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border return true; @@ -72,7 +425,7 @@ bool BicycleCar::drivable(const BicycleCar &bc, double b, double e) const BicycleCar z(*this); // zone border z.h(b); h_d = bc.h() - this->h(); - z.rotate(this->ccr().x(), this->ccr().y(), h_d); + z.rotate(this->ccr(), h_d); // assert z.h() == bc.h() if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border return true; @@ -90,321 +443,171 @@ bool BicycleCar::drivable(const BicycleCar &bc, double b, double e) const return false; } -double BicycleCar::iradi() const +double +BicycleCar::iradi() const { return this->mtr() - this->w() / 2; } -double BicycleCar::ofradi() const +double +BicycleCar::ofradi() const { - return sqrt(pow(this->mtr() + this->w() / 2, 2) + pow(this->df(), 2)); + auto mtrw2 = pow(this->mtr() + this->w() / 2.0, 2.0); + auto df2 = pow(this->df(), 2.0); + return sqrt(mtrw2 + df2); } -double BicycleCar::orradi() const +double +BicycleCar::orradi() const { - return sqrt(pow(this->mtr() + this->w() / 2, 2) + pow(this->dr(), 2)); + auto mtrw2 = pow(this->mtr() + this->w() / 2.0, 2.0); + auto dr2 = pow(this->dr(), 2.0); + return sqrt(mtrw2 + dr2); } -double BicycleCar::perfect_parking_slot_len() const +double +BicycleCar::perfect_parking_slot_len() const { - // see Simon R. Blackburn *The Geometry of Perfect Parking* - // see https://www.ma.rhul.ac.uk/SRBparking - double r = this->ctc() / 2; - double l = this->wb(); - double k = this->df() - this->wb(); - double w = this->w(); - return - this->l() - + sqrt( - (r*r - l*l) - + pow(l + k, 2) - - pow(sqrt(r*r - l*l) - w, 2) - ) - - l - - k - ; + auto r = this->ctc() / 2.0; + auto l = this->wb(); + auto k = this->df() - this->wb(); + auto w = this->w(); + auto r2l2 = r * r - l * l; + auto s = r2l2 + pow(l + k, 2.0) - pow(sqrt(r2l2) - w, 2.0); + return this->len() + sqrt(s) - l - k; } -void BicycleCar::set_max_steer() +void +BicycleCar::set_max_steer() { this->st(atan(this->wb() / this->mtr())); } -// car frame -double BicycleCar::lfx() const +double +BicycleCar::lfx() const { double lfx = this->x(); - lfx += (this->w() / 2) * cos(this->h() + M_PI / 2); + lfx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0); lfx += this->df() * cos(this->h()); - lfx += this->sd() * cos(this->h()); return lfx; } -double BicycleCar::lfy() const +double +BicycleCar::lfy() const { double lfy = this->y(); - lfy += (this->w() / 2) * sin(this->h() + M_PI / 2); + lfy += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0); lfy += this->df() * sin(this->h()); - lfy += this->sd() * sin(this->h()); return lfy; } -double BicycleCar::lrx() const +double +BicycleCar::lrx() const { double lrx = this->x(); - lrx += (this->w() / 2) * cos(this->h() + M_PI / 2); + lrx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0); lrx += -this->dr() * cos(this->h()); - lrx += -this->sd() * cos(this->h()); return lrx; } -double BicycleCar::lry() const +double +BicycleCar::lry() const { double lry = this->y(); - lry += (this->w() / 2) * sin(this->h() + M_PI / 2); + lry += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0); lry += -this->dr() * sin(this->h()); - lry += -this->sd() * sin(this->h()); return lry; } -double BicycleCar::rrx() const +double +BicycleCar::rrx() const { double rrx = this->x(); - rrx += (this->w() / 2) * cos(this->h() - M_PI / 2); + rrx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0); rrx += -this->dr() * cos(this->h()); - rrx += -this->sd() * cos(this->h()); return rrx; } -double BicycleCar::rry() const +double +BicycleCar::rry() const { double rry = this->y(); - rry += (this->w() / 2) * sin(this->h() - M_PI / 2); + rry += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0); rry += -this->dr() * sin(this->h()); - rry += -this->sd() * sin(this->h()); return rry; } -double BicycleCar::rfx() const +double +BicycleCar::rfx() const { double rfx = this->x(); - rfx += (this->w() / 2) * cos(this->h() - M_PI / 2); + rfx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0); rfx += this->df() * cos(this->h()); - rfx += this->sd() * cos(this->h()); return rfx; } -double BicycleCar::rfy() const +double +BicycleCar::rfy() const { double rfy = this->y(); - rfy += (this->w() / 2) * sin(this->h() - M_PI / 2); + rfy += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0); rfy += this->df() * sin(this->h()); - rfy += this->sd() * sin(this->h()); return rfy; } -double BicycleCar::ralx() const +double +BicycleCar::ralx() const { double lrx = this->x(); - lrx += (this->w() / 2) * cos(this->h() + M_PI / 2); + lrx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0); return lrx; } -double BicycleCar::raly() const +double +BicycleCar::raly() const { double lry = this->y(); - lry += (this->w() / 2) * sin(this->h() + M_PI / 2); + lry += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0); return lry; } -double BicycleCar::rarx() const +double +BicycleCar::rarx() const { double rrx = this->x(); - rrx += (this->w() / 2) * cos(this->h() - M_PI / 2); + rrx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0); return rrx; } -double BicycleCar::rary() const +double +BicycleCar::rary() const { double rry = this->y(); - rry += (this->w() / 2) * sin(this->h() - M_PI / 2); + rry += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0); return rry; } -BicycleCar BicycleCar::ccl() const +Point +BicycleCar::ccl() const { - BicycleCar bc; - bc.x(this->x() + this->mtr() * cos(this->h() + M_PI / 2)); - bc.y(this->y() + this->mtr() * sin(this->h() + M_PI / 2)); - bc.h(this->h()); - return bc; + return Point( + this->x() + this->mtr() * cos(this->h() + M_PI / 2.0), + this->y() + this->mtr() * sin(this->h() + M_PI / 2.0) + ); } -BicycleCar BicycleCar::ccr() const +Point +BicycleCar::ccr() const { - BicycleCar bc; - bc.x(this->x() + this->mtr() * cos(this->h() - M_PI / 2)); - bc.y(this->y() + this->mtr() * sin(this->h() - M_PI / 2)); - bc.h(this->h()); - return bc; + return Point( + this->x() + this->mtr() * cos(this->h() - M_PI / 2.0), + this->y() + this->mtr() * sin(this->h() - M_PI / 2.0) + ); } -// moving -void BicycleCar::next() +void +BicycleCar::next() { this->x(this->x() + this->sp() * cos(this->h())); this->y(this->y() + this->sp() * sin(this->h())); this->h(this->h() + this->sp() / this->wb() * tan(this->st())); } - -void BicycleCar::rotate(double cx, double cy, double angl) -{ - double px = this->x(); - double py = this->y(); - px -= cx; - py -= cy; - double nx = px * cos(angl) - py * sin(angl); - double ny = px * sin(angl) + py * cos(angl); - this->h(this->h() + angl); - this->x(nx + cx); - this->y(ny + cy); -} - -BicycleCar::BicycleCar() -{ - // TODO according to mtr_ FIXME - this->mtr_ = sqrt( - pow(10.82 / 2, 2) - - pow(this->wb(), 2) - ) - - this->w() / 2 - ; -} - -std::tuple -collide( - std::vector> &p1, - std::vector> &p2 -) -{ - for (unsigned int i = 0; i < p1.size() - 1; i++) { - for (unsigned int j = 0; j < p2.size() - 1; j++) { - auto x = intersect( - std::get<0>(p1[i]), - std::get<1>(p1[i]), - std::get<0>(p1[i + 1]), - std::get<1>(p1[i + 1]), - std::get<0>(p2[j]), - std::get<1>(p2[j]), - std::get<0>(p2[j + 1]), - std::get<1>(p2[j + 1]) - ); - if (std::get<0>(x)) - return std::make_tuple(true, i, j); - } - } - return std::make_tuple(false, 0, 0); -} - -bool -inside(double x, double y, std::vector> &poly) -{ - unsigned int i = 0; - unsigned int j = 3; - bool inside = false; - for (i = 0; i < 4; i++) { - if ( - (std::get<1>(poly[i]) > y) != (std::get<1>(poly[j]) > y) - && ( - x < std::get<0>(poly[i]) - + (std::get<0>(poly[j]) - std::get<0>(poly[i])) - * (y - std::get<1>(poly[i])) - / (std::get<1>(poly[j]) - std::get<1>(poly[i])) - ) - ) - inside = !inside; - j = i; - } - return inside; -} - -std::tuple -intersect( - double x1, double y1, - double x2, double y2, - double x3, double y3, - double x4, double y4 -) -{ - double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4); - if (deno == 0) - return std::make_tuple(false, 0, 0); - double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4); - t /= deno; - double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3); - u *= -1; - u /= deno; - if (t < 0 || t > 1 || u < 0 || u > 1) - return std::make_tuple(false, 0, 0); - return std::make_tuple(true, x1 + t * (x2 - x1), y1 + t * (y2 - y1)); -} - -std::tuple -intersect( - double cx, double cy, double r, - double x1, double y1, - double x2, double y2 -) { - x2 -= cx; - x1 -= cx; - y2 -= cy; - y1 -= cy; - if (y1 == y2) - y1 += 0.00001; - double dx = x2 - x1; - double dy = y2 - y1; - double dr = sqrt(dx*dx + dy*dy); - double D = x1*y2 - x2*y1; - if (r*r * dr*dr - D*D < 0) - return std::make_tuple(false, 0, 0, 0, 0); - // intersection coordinates - double ix1 = (D*dy + sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr); - ix1 += cx; - double ix2 = (D*dy - sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr); - ix2 += cx; - double iy1 = (-D*dx + std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr); - iy1 += cy; - double iy2 = (-D*dx - std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr); - iy2 += cy; - return std::make_tuple(true, ix1, iy1, ix2, iy2); -} - -double -angle_between_three_points( - double x1, double y1, - double x2, double y2, - double x3, double y3 -) { - double d1x = x2 - x1; - double d1y = y2 - y1; - double d2x = x3 - x2; - double d2y = y3 - y2; - - double dot = d1x*d2x + d1y*d2y; - double d1 = sqrt(d1x*d1x + d1y*d1y); - double d2 = sqrt(d2x*d2x + d2y*d2y); - - double delta = acos(dot / (d1 * d2)); - return std::min(delta, M_PI - delta); -} - -bool -right_side_of_line( - double x1, double y1, - double x2, double y2, - double x3, double y3 -) { - if (sgn((x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1)) < 0) - return false; - else - return true; -} -- 2.39.2