+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
/*! \file */
#ifndef BCAR_BCAR_H
#define BCAR_BCAR_H
return (T(0) < val) - (val < T(0));
}
+class Line;
+
class Point {
private:
- double x_ = 0.0;
- double y_ = 0.0;
+ double _x = 0.0;
+ double _y = 0.0;
public:
- Point(double x, double y);
Point();
+ Point(double x, double y);
/*! Get horizontal coordinate. */
double x() const;
* \param poly Polygon to consider.
*/
bool inside_of(std::vector<Point> const& poly) const;
+
+ /*! \brief Return `true` if `this` point is inside the circle `c`, `r`.
+ *
+ * \see * https://math.stackexchange.com/questions/198764/how-to-know-if-a-point-is-inside-a-circle#198769
+ */
+ bool inside_of(Point const& c, double const r) 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 Translate self.
+ *
+ * \param p `Point` offset to translate by.
+ */
+ void translate(Point const& p);
+
+ /*! \brief Rotate self around the point.
+
+ \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;
+
+ /*! Generate output for plotting with gnuplot. */
+ void gen_gnuplot_to(std::ostream& out);
+
+ bool operator==(Point const& p);
+ friend std::ostream& operator<<(std::ostream& out, Point const& p);
};
class Line {
private:
- Point first;
- Point last;
- Point intersection1;
- Point intersection2;
+ Point _b;
+ Point _e;
+ Point _i1;
+ Point _i2;
public:
Line(Point const& fp, Point const& lp);
- /*! Get first point. */
- Point fp() const&;
+ /*! Get beginning point. */
+ Point b() const&;
+
+ /*! Get end point. */
+ Point e() const&;
- /*! Get last point. */
- Point lp() const&;
+ /*! Get middle point. */
+ Point m() const;
/*! Get intersection point. */
- Point in1() const&;
+ Point i1() const&;
/*! Get intersection point. */
- Point in2() const&;
+ Point i2() const&;
/*! \brief Return if `this` line intersects with line `li`.
*
* If the method returns `true`, the intersection `Point` is available
- * in `this->in1()`.
+ * in `this->i1()`.
*
* \see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
*
/*! \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()`.
+ * in `this->i1()` and `this->i2()`.
*
* \see https://mathworld.wolfram.com/Circle-LineIntersection.html
*
*/
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;
+ double len() const;
+
+ double h() const;
+
+ /*! Generate output for plotting with gnuplot. */
+ void gen_gnuplot_to(std::ostream& out);
+
+ friend std::ostream& operator<<(std::ostream& out, Line const& li);
};
/*! Store coordinates `x`, `y`, and heading `h`. */
-class Pose {
+class Pose : public virtual Point {
private:
- double x_ = 0.0;
- double y_ = 0.0;
- double h_ = 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);
+ 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);
- /*! \brief Rotate self around the point.
+ /*! Set pose (`x`, `y`, and `h`.) */
+ void set_pose(Pose const& p);
- \param c Rotation center `Point`.
- \param angl Angle of rotation.
- */
void rotate(Point const& c, double const angl);
+ void reflect(Line const& li);
+
+ bool operator==(Pose const& p);
friend std::ostream& operator<<(std::ostream& out, Pose const& p);
};
-class PoseRange : public Pose {
+class PoseRange : public virtual Pose {
private:
- double e_ = 0.0;
- using Pose::h;
+ Pose _bp;
+ Pose _ep;
+ void set_xyh();
public:
+ PoseRange(Pose bp, Pose ep);
+ PoseRange(double x, double y, double b, double e);
+
+ Pose bp() const;
+ Pose ep() const;
+
/*! 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 translate(Point const& p);
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
+ * - The default is Renault ZOE (www.car.info)
*/
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;
+ double _curb_to_curb = 10.802166641822163;
+ double _width_with_mirrors = 1.945;
+ double _width = 1.771;
+ double _wheelbase = 2.588;
+ double _distance_to_front = 3.427;
+ double _length = 4.084;
+ double _front_track = 1.511;
public:
/*! Get curb-to-curb distance. */
double ctc() const;
/*! Set width. */
void w(double w);
+ /*! Get width with mirrors. */
+ double wwm() const;
+
+ /*! Set width with mirrors. */
+ void wwm(double w);
+
/*! Get length. */
double len() const;
/*! Get distance from rear axle to rear. */
double dr() const;
+ /*! Set front track. */
+ void ft(double ft);
+
+ /*! Get front track. */
+ double ft() 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
+ * 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.
+ * _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
+ * 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 p `PoseRange` (resp. `Pose`) to achieve.
- */
- bool drivable(PoseRange const& p) const;
- bool drivable(Pose const& p) const;
/*! \brief Return inner radius.
*
*/
double orradi() const;
+ /*! \brief Return inner mirror radius.
+ *
+ * The inner mirror radius is the distance from minimum turning radius
+ * circle center to the farther mirror (from the rear axle view).
+ */
+ double imradi() const;
+
+ /*! \brief Return outer mirror radius.
+ *
+ * The outer mirror radius is the distance from minimum turning radius
+ * circle center to the farther mirror (from the rear axle view).
+ */
+ double omradi() const;
+
/*! \brief Return length of perfect parking slot.
*
* The width of the slot is the same as the width of the car.
* \see https://www.ma.rhul.ac.uk/SRBparking
*/
double perfect_parking_slot_len() 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 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();
/*! Get frame's right front y coordinate. */
double rfy() const;
- /*! Get rear axle's left x coordinate. */
- double ralx() 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 y coordinate. */
- double raly() const;
+ /*! Get frame's left rear axle x coordinate. */
+ double lrax() const;
- /*! Get rear axle's right x coordinate. */
- double rarx() const;
+ /*! Get frame's left rear axle y coordinate. */
+ double lray() const;
- /*! Get rear axle's right y coordinate. */
- double rary() const;
+ /*! Get frame's right rear axle x coordinate. */
+ double rrax() const;
+
+ /*! Get frame's right rear axle y coordinate. */
+ double rray() const;
+
+ /*! Get frame's left rear axle point. */
+ Point lra() const;
+
+ /*! Get frame's right rear axle point. */
+ Point rra() const;
+
+ /*! Get frame's left front axle x coordinate. */
+ double lfax() const;
+
+ /*! Get frame's left front axle y coordinate. */
+ double lfay() const;
+
+ /*! Get frame's right front axle x coordinate. */
+ double rfax() const;
+
+ /*! Get frame's right front axle y coordinate. */
+ double rfay() const;
+
+ /*! Get iframe's left front axle point. */
+ Point lfa() const;
+
+ /*! Get frame's right front axle point. */
+ Point rfa() const;
+
+ /*! Get frame's left front mirror x coordinate. */
+ double lfmx() const;
+
+ /*! Get frame's left front mirror y coordinate. */
+ double lfmy() const;
+
+ /*! Get frame's right front mirror x coordinate. */
+ double rfmx() const;
+
+ /*! Get frame's right front mirror y coordinate. */
+ double rfmy() const;
+
+ /*! Get iframe's left front mirror point. */
+ Point lfm() const;
+
+ /*! Get frame's right front mirror point. */
+ Point rfm() const;
+
+ /*! Get frame's center front x coordinate. */
+ double cfx() const;
+
+ /*! Get frame's center front y coordinate. */
+ double cfy() const;
+
+ /*! Get frame's center front point. */
+ Point cf() const;
/*! Min. turning radius circle center on left. */
Point ccl() const;
/*! Next car position based on speed `sp` and steer `st`. */
void next();
+
+ /*! Options for generating output for gnuplot. */
+ class GenPlotOpts {
+ public:
+ bool LF_POINT = false;
+ bool LR_POINT = false;
+ bool RR_POINT = false;
+ bool RF_POINT = false;
+ bool LFM_POINT = false;
+ bool RFM_POINT = false;
+ bool CRA_POINT = false;
+ bool CAR_POINT = false;
+ bool LRA_POINT = false;
+ bool RRA_POINT = false;
+
+ bool LEFT = false;
+ bool RIGHT = false;
+ bool REAR = false;
+ bool FRONT = false;
+ bool FRAME = false; // LEFT, RIGHT, REAR, FRONT
+ bool ARROW = false;
+ bool CROSS = false;
+ bool CAR = false; // CROSS, ARROW, FRAME
+ bool LEFT_MIRROR = false;
+ bool RIGHT_MIRROR = false;
+ bool MIRRORS = false; // RIGHT_MIRROR, LEFT_MIRROR
+ bool ALL = true; // MIRRORS, CAR
+ };
+
+ /*! Generate output for plotting with gnuplot. */
+ void gen_gnuplot_to(std::ostream& out, GenPlotOpts opts);
};
} // namespace bcar