/*! \file */
-#ifndef BCAR_H
-#define BCAR_H
+#ifndef BCAR_BCAR_H
+#define BCAR_BCAR_H
-#include <cmath>
#include <ostream>
-#include <tuple>
#include <vector>
-/*! \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 <typename T> int sgn(T val) {
+ return (T(0) < val) - (val < T(0));
+}
+
+class Line;
+
+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();
+ Point(double x, double y);
+
+ /*! 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);
- When `false` is returned the `bc` may still be drivable,
- because only "line segment - circle arc - line segment"
- paths are considered in ``drivable`` method.
+ /*! \brief Return the smallest angle between three points.
- \param[in] bc The bicycle car to achieve.
+ \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<Point> const& poly) const;
+
+ /*! \brief Return `true` if on the right side of the plane.
+ *
+ * The plane is given by the line `li`, where `li->b()` is the base
+ * point and the direction is given by `li->e() - li->b()`.
+ *
+ * \param li The plane to consider is given by `li`.
+ */
+ bool on_right_side_of(Line const& li) const;
+
+ /*! \brief Rotate self around the point.
- 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.
+ \param c Rotation center `Point`.
+ \param angl Angle of rotation.
*/
+ void rotate(Point const& c, double const angl);
+
+ /*! \brief Compute reflection of `this` around the `Line`.
+ *
+ * \param li The plane to reflect around is given by `li`.
+ */
+ void reflect(Line const& li);
+
+ /*! Return Euclidean distance to `p`. */
+ double edist(Point const& p) const;
+
+ friend std::ostream& operator<<(std::ostream& out, Point const& p);
+};
+
+class Line {
+private:
+ Point b_;
+ Point e_;
+ Point i1_;
+ Point i2_;
+public:
+ Line(Point const& fp, Point const& lp);
+
+ /*! Get beginning point. */
+ Point b() const&;
+
+ /*! Get end point. */
+ Point e() const&;
+
+ /*! Get intersection point. */
+ Point i1() const&;
+
+ /*! Get intersection point. */
+ Point i2() const&;
+
+ /*! \brief Return if `this` line intersects with line `li`.
+ *
+ * If the method returns `true`, the intersection `Point` is available
+ * in `this->i1()`.
+ *
+ * \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->i1()` and `this->i2()`.
+ *
+ * \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);
+
+ double len() const;
+
+ double h() const;
+
+ friend std::ostream& operator<<(std::ostream& out, Line const& li);
+};
+
+/*! Store coordinates `x`, `y`, and heading `h`. */
+class Pose : public virtual Point {
+private:
+ double h_ = 0.0;
+public:
+ using Point::Point;
+ Pose(double x, double y, double h);
+
+ /*! Get heading in the interval [-pi, +pi] radians. */
+ double h() const;
+
+ /*! Set heading in radians. It's recomputed to [-pi, +pi]. */
+ void h(double h);
+
+ /*! Set pose (`x`, `y`, and `h`.) */
+ void set_pose(Pose const& p);
+
+ void rotate(Point const& c, double const angl);
+
+ void reflect(Line const& li);
+
+ friend std::ostream& operator<<(std::ostream& out, Pose const& p);
+};
+
+class PoseRange : public virtual Pose {
+private:
+ double e_ = 0.0;
+ using Pose::h;
+public:
+ /*! Get heading's begin in the interval [-pi, +pi] radians. */
+ double b() const;
+
+ /*! Set heading's begin in radians. It's recomputed to [-pi, +pi]. */
+ void b(double b);
+
+ /*! Get heading's end in the interval [-pi, +pi] radians. */
+ double e() const;
+
+ /*! Set heading's end in radians. It's recomputed to [-pi, +pi]. */
+ void e(double e);
+
+ void rotate(Point const& c, double const angl);
+
+ void reflect(Line const& li);
+
+ friend std::ostream& operator<<(std::ostream& out, PoseRange 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 rear 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;
+
+ /*! \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.
- */
+};
+
+/*! 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 virtual Pose, public virtual CarSize,
+ public virtual CarMove {
+private:
+public:
+ /*! \brief Return `true` if `this` can drive to `p` trivially.
+ *
+ * Trivially means that `this` can drive to `p` by line segment - circle
+ * arc - line segment.
+ *
+ * \param p `PoseRange` (resp. `Pose`) to achieve.
+ */
+ bool drivable(PoseRange const& p) const;
+ bool drivable(Pose const& p) const;
+
+ /*! 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 frame's left front point. */
+ Point lf() const;
+
+ /*! Get frame's left rear point. */
+ Point lr() const;
+
+ /*! Get frame's right rear point. */
+ Point rr() const;
+
+ /*! Get frame's right front point. */
+ Point rf() const;
+
+ /*! Get frame's left side. */
+ Line left() const;
+
+ /*! Get frame's rear side. */
+ Line rear() const;
+
+ /*! Get frame's right side. */
+ Line right() const;
+
+ /*! Get frame's front side. */
+ Line front() 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<bool, int, int>`, 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<bool, unsigned int, unsigned int>
-collide(
- std::vector<std::tuple<double, double>> &p1,
- std::vector<std::tuple<double, double>> &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<std::tuple<double, double>> &poly);
-
-/*! \brief Return intersection of two line segments.
-
-The output is tuple `std::tuple<bool, double, double>`, 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<bool, double, double>
-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<bool, double, double, double, double>`, 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<bool, double, double, double, double>
-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 */
projects / hubacji1 / bcar.git / blobdiff