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 on the right side of the plane.
60 * The plane is given by the line `li`, where `li->b()` is the base
61 * point and the direction is given by `li->e() - li->b()`.
63 * \param li The plane to consider is given by `li`.
65 bool on_right_side_of(Line const& li) const;
67 /*! \brief Translate self.
69 * \param p `Point` offset to translate by.
71 void translate(Point const& p);
73 /*! \brief Rotate self around the point.
75 \param c Rotation center `Point`.
76 \param angl Angle of rotation.
78 void rotate(Point const& c, double const angl);
80 /*! \brief Compute reflection of `this` around the `Line`.
82 * \param li The plane to reflect around is given by `li`.
84 void reflect(Line const& li);
86 /*! Return Euclidean distance to `p`. */
87 double edist(Point const& p) const;
89 bool operator==(Point const& p);
90 friend std::ostream& operator<<(std::ostream& out, Point const& p);
100 Line(Point const& fp, Point const& lp);
102 /*! Get beginning point. */
105 /*! Get end point. */
108 /*! Get middle point. */
111 /*! Get intersection point. */
114 /*! Get intersection point. */
117 /*! \brief Return if `this` line intersects with line `li`.
119 * If the method returns `true`, the intersection `Point` is available
122 * \see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
124 * \param li The line to check the intersection with.
126 bool intersects_with(Line const& li);
128 /*! \brief Return intersections of `this` (infinite) line and circle.
130 * If the method returns `true`, the intersection `Point`s are available
131 * in `this->i1()` and `this->i2()`.
133 * \see https://mathworld.wolfram.com/Circle-LineIntersection.html
135 * \param c Circle center.
136 * \param r Circle radius.
138 bool intersects_with(Point const& c, double const r);
144 friend std::ostream& operator<<(std::ostream& out, Line const& li);
147 /*! Store coordinates `x`, `y`, and heading `h`. */
148 class Pose : public virtual Point {
153 Pose(double x, double y, double h);
155 /*! Get heading in the interval [-pi, +pi] radians. */
158 /*! Set heading in radians. It's recomputed to [-pi, +pi]. */
161 /*! Set pose (`x`, `y`, and `h`.) */
162 void set_pose(Pose const& p);
164 void rotate(Point const& c, double const angl);
166 void reflect(Line const& li);
168 bool operator==(Pose const& p);
169 friend std::ostream& operator<<(std::ostream& out, Pose const& p);
172 class PoseRange : public virtual Pose {
178 PoseRange(Pose bp, Pose ep);
179 PoseRange(double x, double y, double b, double e);
184 /*! Get heading's begin in the interval [-pi, +pi] radians. */
187 /*! Get heading's end in the interval [-pi, +pi] radians. */
190 void translate(Point const& p);
191 void rotate(Point const& c, double const angl);
192 void reflect(Line const& li);
194 friend std::ostream& operator<<(std::ostream& out, PoseRange const& p);
197 /*! \brief Store car size.
199 * - Default is https://en.wikipedia.org/wiki/Fiat_Punto
203 double curb_to_curb_ = 10.820;
204 double width_ = 1.625;
205 double wheelbase_ = 2.450;
206 double distance_to_front_ = 3.105;
207 double length_ = 3.760;
208 double _front_track = 1.625;
210 /*! Get curb-to-curb distance. */
213 /*! Set curb-to-curb distance. */
214 void ctc(double ctc);
216 /*! Get wheelbase. */
219 /*! Set wheelbase. */
232 void len(double len);
234 /*! Get distance from rear axle to front. */
237 /*! Set distance from rear axle to front. */
240 /*! Get distance from rear axle to rear. */
243 /*! Set front track. */
246 /*! Get front track. */
249 /*! \brief Get minimum turning radius.
251 * Please, note that the method returns really _minimum turning radius_,
252 * which is the distance from the rear axle center to the center of
253 * left or right rotation given by the kinematics constrants, i.e.
254 * _wheelbase_ and _curb-to-curb_ distance.
256 * Sometimes _minimum turning radius_ is not radius, not minimum, or not
257 * turning. In this method, _minimum turning radius_ is minimum turning
262 /*! \brief Return inner radius.
264 * The inner radius is the distance from minimum turning radius circle
265 * center to the nearest point on the car. In this case, the nearest
266 * points on the car are rear axle endpoints.
268 double iradi() const;
270 /*! \brief Return outer front radius.
272 * The outer front radius is the distance from minimum turning radius
273 * circle center to the farthest point on the front (from the rear axle
274 * view) part of the car.
276 double ofradi() const;
278 /*! \brief Return outer rear radius.
280 * The outer rear radius is the distance from minimum turning radius
281 * circle center to the farthest point on the rear (from the rear axle
282 * view) part of the car.
284 double orradi() const;
286 /*! \brief Return length of perfect parking slot.
288 * The width of the slot is the same as the width of the car.
290 * \see Simon R. Blackburn *The Geometry of Perfect Parking*
291 * \see https://www.ma.rhul.ac.uk/SRBparking
293 double perfect_parking_slot_len() const;
296 /*! Store car motion. */
315 /*! \brief Geometrical computations of a bicycle car.
317 * - `x()` and `y()` methods returns coordinates of rear axle center.
319 class BicycleCar : public virtual Pose, public virtual CarSize,
320 public virtual CarMove {
323 /*! \brief Return `true` if `this` can drive to `p` trivially.
325 * Trivially means that `this` can drive to `p` by line segment - circle
326 * arc - line segment.
328 * \param p `PoseRange` (resp. `Pose`) to achieve.
330 bool drivable(PoseRange const& p) const;
331 bool drivable(Pose const& p) const;
333 /*! Set maximum steering angle. */
334 void set_max_steer();
336 /*! Get frame's left front x coordinate. */
339 /*! Get frame's left front y coordinate. */
342 /*! Get frame's left rear x coordinate. */
345 /*! Get frame's left rear y coordinate. */
348 /*! Get frame's right rear x coordinate. */
351 /*! Get frame's right rear y coordinate. */
354 /*! Get frame's right front x coordinate. */
357 /*! Get frame's right front y coordinate. */
360 /*! Get frame's left front point. */
363 /*! Get frame's left rear point. */
366 /*! Get frame's right rear point. */
369 /*! Get frame's right front point. */
372 /*! Get frame's left side. */
375 /*! Get frame's rear side. */
378 /*! Get frame's right side. */
381 /*! Get frame's front side. */
384 /*! Get rear axle's left x coordinate. */
387 /*! Get rear axle's left y coordinate. */
390 /*! Get rear axle's right x coordinate. */
393 /*! Get rear axle's right y coordinate. */
396 /*! Min. turning radius circle center on left. */
399 /*! Min. turning radius circle center on rigth. */
402 /*! Next car position based on speed `sp` and steer `st`. */
407 #endif /* BCAR_BCAR_H */