10 template <typename T> int sgn(T val) {
11 return (T(0) < val) - (val < T(0));
21 Point(double x, double y);
24 /*! Get horizontal coordinate. */
27 /*! Set horizontal coordinate. */
30 /*! Get vertical coordinate. */
33 /*! Set vertical coordinate. */
36 /*! \brief Return the smallest angle between three points.
38 \see https://math.stackexchange.com/questions/361412/finding-the-angle-between-three-points
40 double min_angle_between(Point const& p1, Point const& p2) const;
42 /*! \brief Return `true` if `this` point is inside of polygon `poly`.
44 * The polygon is given by the vector of `Point`s.
46 * \see https://en.wikipedia.org/wiki/Even%E2%80%93odd_rule
48 * \param poly Polygon to consider.
50 bool inside_of(std::vector<Point> const& poly) const;
52 /*! \brief Return `true` if on the right side of the plane.
54 * The plane is given by the line `li`, where `li->fp()` is the base
55 * point and the direction is given by `li->lp() - li->fp()`.
57 * \param li The plane to consider is given by `li`.
59 bool on_right_side_of(Line const& li) const;
61 /*! \brief Rotate self around the point.
63 \param c Rotation center `Point`.
64 \param angl Angle of rotation.
66 void rotate(Point const& c, double const angl);
68 /*! Return Euclidean distance to `p`. */
69 double edist(Point const& p) const;
71 friend std::ostream& operator<<(std::ostream& out, Point const& p);
81 Line(Point const& fp, Point const& lp);
83 /*! Get first point. */
86 /*! Get last point. */
89 /*! Get intersection point. */
92 /*! Get intersection point. */
95 /*! \brief Return if `this` line intersects with line `li`.
97 * If the method returns `true`, the intersection `Point` is available
100 * \see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
102 * \param li The line to check the intersection with.
104 bool intersects_with(Line const& li);
106 /*! \brief Return intersections of `this` (infinite) line and circle.
108 * If the method returns `true`, the intersection `Point`s are available
109 * in `this->in1()` and `this->in2()`.
111 * \see https://mathworld.wolfram.com/Circle-LineIntersection.html
113 * \param c Circle center.
114 * \param r Circle radius.
116 bool intersects_with(Point const& c, double const r);
120 friend std::ostream& operator<<(std::ostream& out, Line const& li);
123 /*! Store coordinates `x`, `y`, and heading `h`. */
124 class Pose : public virtual Point {
128 Pose(double x, double y, double h);
131 /*! Get heading in the interval [-pi, +pi] radians. */
134 /*! Set heading in radians. It's recomputed to [-pi, +pi]. */
137 /*! Set pose (`x`, `y`, and `h`.) */
138 void set_pose(Pose const& p);
140 void rotate(Point const& c, double const angl);
142 friend std::ostream& operator<<(std::ostream& out, Pose const& p);
145 class PoseRange : public virtual Pose {
150 /*! Get heading's begin in the interval [-pi, +pi] radians. */
153 /*! Set heading's begin in radians. It's recomputed to [-pi, +pi]. */
156 /*! Get heading's end in the interval [-pi, +pi] radians. */
159 /*! Set heading's end in radians. It's recomputed to [-pi, +pi]. */
162 void rotate(Point const& c, double const angl);
164 friend std::ostream& operator<<(std::ostream& out, PoseRange const& p);
167 /*! \brief Store car size.
169 * - Default is https://en.wikipedia.org/wiki/Fiat_Punto
173 double curb_to_curb = 10.820;
174 double width = 1.625;
175 double wheelbase = 2.450;
176 double distance_to_front = 3.105;
177 double length = 3.760;
179 /*! Get curb-to-curb distance. */
182 /*! Set curb-to-curb distance. */
183 void ctc(double ctc);
185 /*! Get wheelbase. */
188 /*! Set wheelbase. */
201 void len(double len);
203 /*! Get distance from rear axle to front. */
206 /*! Set distance from rear axle to front. */
209 /*! Get distance from rear axle to rear. */
212 /*! \brief Get minimum turning radius.
214 * Please, note that the method returns really _minimum turning radius_,
215 * which is the distance from the reare axle center to the center of
216 * left or right rotation given by the kinematics constrants, i.e.
217 * _wheelbase_ and _curb-to-curb_ distance.
219 * Sometimes _minimum turning radius_ is not radius, not minimum, or not
220 * turning. In this method, _minimum turning radius_ is minimum turning
225 /*! \brief Return inner radius.
227 * The inner radius is the distance from minimum turning radius circle
228 * center to the nearest point on the car. In this case, the nearest
229 * points on the car are rear axle endpoints.
231 double iradi() const;
233 /*! \brief Return outer front radius.
235 * The outer front radius is the distance from minimum turning radius
236 * circle center to the farthest point on the front (from the rear axle
237 * view) part of the car.
239 double ofradi() const;
241 /*! \brief Return outer rear radius.
243 * The outer rear radius is the distance from minimum turning radius
244 * circle center to the farthest point on the rear (from the rear axle
245 * view) part of the car.
247 double orradi() const;
249 /*! \brief Return length of perfect parking slot.
251 * The width of the slot is the same as the width of the car.
253 * \see Simon R. Blackburn *The Geometry of Perfect Parking*
254 * \see https://www.ma.rhul.ac.uk/SRBparking
256 double perfect_parking_slot_len() const;
259 /*! Store car motion. */
278 /*! \brief Geometrical computations of a bicycle car.
280 * - `x()` and `y()` methods returns coordinates of rear axle center.
282 class BicycleCar : public virtual Pose, public virtual CarSize,
283 public virtual CarMove {
286 /*! \brief Return `false` if `bc` is not achievable.
288 * When `false` is returned the `bc` may still be drivable, but not
289 * trivially, i.e. by "line segment - circle arc - line segment".
291 * \param p `PoseRange` (resp. `Pose`) to achieve.
293 bool drivable(PoseRange const& p) const;
294 bool drivable(Pose const& p) const;
296 /*! Set maximum steering angle. */
297 void set_max_steer();
299 /*! Get frame's left front x coordinate. */
302 /*! Get frame's left front y coordinate. */
305 /*! Get frame's left rear x coordinate. */
308 /*! Get frame's left rear y coordinate. */
311 /*! Get frame's right rear x coordinate. */
314 /*! Get frame's right rear y coordinate. */
317 /*! Get frame's right front x coordinate. */
320 /*! Get frame's right front y coordinate. */
323 /*! Get frame's left front point. */
326 /*! Get frame's left rear point. */
329 /*! Get frame's right rear point. */
332 /*! Get frame's right front point. */
335 /*! Get frame's left side. */
338 /*! Get frame's rear side. */
341 /*! Get frame's right side. */
344 /*! Get frame's front side. */
347 /*! Get rear axle's left x coordinate. */
350 /*! Get rear axle's left y coordinate. */
353 /*! Get rear axle's right x coordinate. */
356 /*! Get rear axle's right y coordinate. */
359 /*! Min. turning radius circle center on left. */
362 /*! Min. turning radius circle center on rigth. */
365 /*! Next car position based on speed `sp` and steer `st`. */
370 #endif /* BCAR_BCAR_H */