2 * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
4 * SPDX-License-Identifier: GPL-3.0-only
16 template <typename T> int sgn(T val) {
17 return (T(0) < val) - (val < T(0));
28 Point(double x, double y);
30 /*! Get horizontal coordinate. */
33 /*! Set horizontal coordinate. */
36 /*! Get vertical coordinate. */
39 /*! Set vertical coordinate. */
42 /*! \brief Return the smallest angle between three points.
44 \see https://math.stackexchange.com/questions/361412/finding-the-angle-between-three-points
46 double min_angle_between(Point const& p1, Point const& p2) const;
48 /*! \brief Return `true` if `this` point is inside of polygon `poly`.
50 * The polygon is given by the vector of `Point`s.
52 * \see https://en.wikipedia.org/wiki/Even%E2%80%93odd_rule
54 * \param poly Polygon to consider.
56 bool inside_of(std::vector<Point> const& poly) const;
58 /*! \brief Return `true` if `this` point is inside the circle `c`, `r`.
60 * \see * https://math.stackexchange.com/questions/198764/how-to-know-if-a-point-is-inside-a-circle#198769
62 bool inside_of(Point const& c, double const r) const;
64 /*! \brief Return `true` if on the right side of the plane.
66 * The plane is given by the line `li`, where `li->b()` is the base
67 * point and the direction is given by `li->e() - li->b()`.
69 * \param li The plane to consider is given by `li`.
71 bool on_right_side_of(Line const& li) const;
73 /*! \brief Translate self.
75 * \param p `Point` offset to translate by.
77 void translate(Point const& p);
79 /*! \brief Rotate self around the point.
81 \param c Rotation center `Point`.
82 \param angl Angle of rotation.
84 void rotate(Point const& c, double const angl);
86 /*! \brief Compute reflection of `this` around the `Line`.
88 * \param li The plane to reflect around is given by `li`.
90 void reflect(Line const& li);
92 /*! Return Euclidean distance to `p`. */
93 double edist(Point const& p) const;
95 /*! Generate output for plotting with gnuplot. */
96 void gen_gnuplot_to(std::ostream& out);
98 bool operator==(Point const& p);
99 friend std::ostream& operator<<(std::ostream& out, Point const& p);
109 Line(Point const& fp, Point const& lp);
111 /*! Get beginning point. */
114 /*! Get end point. */
117 /*! Get middle point. */
120 /*! Get intersection point. */
123 /*! Get intersection point. */
126 /*! \brief Return if `this` line intersects with line `li`.
128 * If the method returns `true`, the intersection `Point` is available
131 * \see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
133 * \param li The line to check the intersection with.
135 bool intersects_with(Line const& li);
137 /*! \brief Return intersections of `this` (infinite) line and circle.
139 * If the method returns `true`, the intersection `Point`s are available
140 * in `this->i1()` and `this->i2()`.
142 * \see https://mathworld.wolfram.com/Circle-LineIntersection.html
144 * \param c Circle center.
145 * \param r Circle radius.
147 bool intersects_with(Point const& c, double const r);
153 /*! Generate output for plotting with gnuplot. */
154 void gen_gnuplot_to(std::ostream& out);
156 friend std::ostream& operator<<(std::ostream& out, Line const& li);
159 /*! Store coordinates `x`, `y`, and heading `h`. */
160 class Pose : public virtual Point {
165 Pose(double x, double y, double h);
167 /*! Get heading in the interval [-pi, +pi] radians. */
170 /*! Set heading in radians. It's recomputed to [-pi, +pi]. */
173 /*! Set pose (`x`, `y`, and `h`.) */
174 void set_pose(Pose const& p);
176 void rotate(Point const& c, double const angl);
178 void reflect(Line const& li);
180 bool operator==(Pose const& p);
181 friend std::ostream& operator<<(std::ostream& out, Pose const& p);
184 class PoseRange : public virtual Pose {
190 PoseRange(Pose bp, Pose ep);
191 PoseRange(double x, double y, double b, double e);
196 /*! Get heading's begin in the interval [-pi, +pi] radians. */
199 /*! Get heading's end in the interval [-pi, +pi] radians. */
202 void translate(Point const& p);
203 void rotate(Point const& c, double const angl);
204 void reflect(Line const& li);
206 friend std::ostream& operator<<(std::ostream& out, PoseRange const& p);
209 /*! \brief Store car size.
211 * - The default is Renault ZOE (www.car.info)
215 double _curb_to_curb = 10.802166641822163;
216 double _width_with_mirrors = 1.945;
217 double _width = 1.771;
218 double _wheelbase = 2.588;
219 double _distance_to_front = 3.427;
220 double _length = 4.084;
221 double _front_track = 1.511;
223 /*! Get curb-to-curb distance. */
226 /*! Set curb-to-curb distance. */
227 void ctc(double ctc);
229 /*! Get wheelbase. */
232 /*! Set wheelbase. */
241 /*! Get width with mirrors. */
244 /*! Set width with mirrors. */
251 void len(double len);
253 /*! Get distance from rear axle to front. */
256 /*! Set distance from rear axle to front. */
259 /*! Get distance from rear axle to rear. */
262 /*! Set front track. */
265 /*! Get front track. */
268 /*! \brief Get minimum turning radius.
270 * Please, note that the method returns really _minimum turning radius_,
271 * which is the distance from the rear axle center to the center of
272 * left or right rotation given by the kinematics constrants, i.e.
273 * _wheelbase and _curb-to-curb_ distance.
275 * Sometimes _minimum turning _radius is not radius, not minimum, or not
276 * turning. In this method, _minimum turning _radius is minimum turning
281 /*! \brief Return inner radius.
283 * The inner radius is the distance from minimum turning radius circle
284 * center to the nearest point on the car. In this case, the nearest
285 * points on the car are rear axle endpoints.
287 double iradi() const;
289 /*! \brief Return outer front radius.
291 * The outer front radius is the distance from minimum turning radius
292 * circle center to the farthest point on the front (from the rear axle
293 * view) part of the car.
295 double ofradi() const;
297 /*! \brief Return outer rear radius.
299 * The outer rear radius is the distance from minimum turning radius
300 * circle center to the farthest point on the rear (from the rear axle
301 * view) part of the car.
303 double orradi() const;
305 /*! \brief Return inner mirror radius.
307 * The inner mirror radius is the distance from minimum turning radius
308 * circle center to the farther mirror (from the rear axle view).
310 double imradi() const;
312 /*! \brief Return outer mirror radius.
314 * The outer mirror radius is the distance from minimum turning radius
315 * circle center to the farther mirror (from the rear axle view).
317 double omradi() const;
319 /*! \brief Return length of perfect parking slot.
321 * The width of the slot is the same as the width of the car.
323 * \see Simon R. Blackburn *The Geometry of Perfect Parking*
324 * \see https://www.ma.rhul.ac.uk/SRBparking
326 double perfect_parking_slot_len() const;
329 /*! Store car motion. */
348 /*! \brief Geometrical computations of a bicycle car.
350 * - `x()` and `y()` methods returns coordinates of rear axle center.
352 class BicycleCar : public virtual Pose, public virtual CarSize,
353 public virtual CarMove {
356 /*! \brief Return `true` if `this` can drive to `p` trivially.
358 * Trivially means that `this` can drive to `p` by line segment - circle
359 * arc - line segment.
361 * \param p `PoseRange` (resp. `Pose`) to achieve.
363 bool drivable(PoseRange const& p) const;
364 bool drivable(Pose const& p) const;
366 /*! Set maximum steering angle. */
367 void set_max_steer();
369 /*! Get frame's left front x coordinate. */
372 /*! Get frame's left front y coordinate. */
375 /*! Get frame's left rear x coordinate. */
378 /*! Get frame's left rear y coordinate. */
381 /*! Get frame's right rear x coordinate. */
384 /*! Get frame's right rear y coordinate. */
387 /*! Get frame's right front x coordinate. */
390 /*! Get frame's right front y coordinate. */
393 /*! Get frame's left front point. */
396 /*! Get frame's left rear point. */
399 /*! Get frame's right rear point. */
402 /*! Get frame's right front point. */
405 /*! Get frame's left side. */
408 /*! Get frame's rear side. */
411 /*! Get frame's right side. */
414 /*! Get frame's front side. */
417 /*! Get frame's left rear axle x coordinate. */
420 /*! Get frame's left rear axle y coordinate. */
423 /*! Get frame's right rear axle x coordinate. */
426 /*! Get frame's right rear axle y coordinate. */
429 /*! Get frame's left rear axle point. */
432 /*! Get frame's right rear axle point. */
435 /*! Get frame's left front axle x coordinate. */
438 /*! Get frame's left front axle y coordinate. */
441 /*! Get frame's right front axle x coordinate. */
444 /*! Get frame's right front axle y coordinate. */
447 /*! Get iframe's left front axle point. */
450 /*! Get frame's right front axle point. */
453 /*! Get frame's left front mirror x coordinate. */
456 /*! Get frame's left front mirror y coordinate. */
459 /*! Get frame's right front mirror x coordinate. */
462 /*! Get frame's right front mirror y coordinate. */
465 /*! Get iframe's left front mirror point. */
468 /*! Get frame's right front mirror point. */
471 /*! Get frame's center front x coordinate. */
474 /*! Get frame's center front y coordinate. */
477 /*! Get frame's center front point. */
480 /*! Min. turning radius circle center on left. */
483 /*! Min. turning radius circle center on rigth. */
486 /*! Next car position based on speed `sp` and steer `st`. */
489 /*! Options for generating output for gnuplot. */
492 bool LF_POINT = false;
493 bool LR_POINT = false;
494 bool RR_POINT = false;
495 bool RF_POINT = false;
496 bool LFM_POINT = false;
497 bool RFM_POINT = false;
498 bool CRA_POINT = false;
499 bool CAR_POINT = false;
500 bool LRA_POINT = false;
501 bool RRA_POINT = false;
507 bool FRAME = false; // LEFT, RIGHT, REAR, FRONT
510 bool CAR = false; // CROSS, ARROW, FRAME
511 bool LEFT_MIRROR = false;
512 bool RIGHT_MIRROR = false;
513 bool MIRRORS = false; // RIGHT_MIRROR, LEFT_MIRROR
514 bool ALL = true; // MIRRORS, CAR
517 /*! Generate output for plotting with gnuplot. */
518 void gen_gnuplot_to(std::ostream& out, GenPlotOpts opts);
522 #endif /* BCAR_BCAR_H */