10 template <typename T> int sgn(T val) {
11 return (T(0) < val) - (val < T(0));
19 Point(double x, double y);
22 /*! Get horizontal coordinate. */
25 /*! Set horizontal coordinate. */
28 /*! Get vertical coordinate. */
31 /*! Set vertical coordinate. */
34 /*! \brief Return the smallest angle between three points.
36 \see https://math.stackexchange.com/questions/361412/finding-the-angle-between-three-points
38 double min_angle_between(Point const& p1, Point const& p2) const;
40 /*! \brief Return `true` if `this` point is inside of polygon `poly`.
42 * The polygon is given by the vector of `Point`s.
44 * \see https://en.wikipedia.org/wiki/Even%E2%80%93odd_rule
46 * \param poly Polygon to consider.
48 bool inside_of(std::vector<Point> const& poly) const;
58 Line(Point const& fp, Point const& lp);
60 /*! Get first point. */
63 /*! Get last point. */
66 /*! Get intersection point. */
69 /*! Get intersection point. */
72 /*! \brief Return if `this` line intersects with line `li`.
74 * If the method returns `true`, the intersection `Point` is available
77 * \see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
79 * \param li The line to check the intersection with.
81 bool intersects_with(Line const& li);
83 /*! \brief Return intersections of `this` (infinite) line and circle.
85 * If the method returns `true`, the intersection `Point`s are available
86 * in `this->in1()` and `this->in2()`.
88 * \see https://mathworld.wolfram.com/Circle-LineIntersection.html
90 * \param c Circle center.
91 * \param r Circle radius.
93 bool intersects_with(Point const& c, double const r);
95 /*! \brief Return if point `p` is on the right side of the plane.
97 * The plane is given by the line `this`, where `this->fp()` is the base
98 * point and the direction is given by `this->lp() - this->fp()`.
100 * \param p The point to consider.
102 bool is_on_right_side(Point const& p) const;
105 /*! Store coordinates `x`, `y`, and heading `h`. */
112 /*! Get horizontal coordinate. */
115 /*! Set horizontal coordinate. */
118 /*! Get vertical coordinate. */
121 /*! Set vertical coordinate. */
124 /*! Get heading in the interval [-pi, +pi] radians. */
127 /*! Set heading in radians. It's recomputed to [-pi, +pi]. */
130 /*! \brief Rotate self around the point.
132 \param c Rotation center `Point`.
133 \param angl Angle of rotation.
135 void rotate(Point const& c, double const angl);
137 friend std::ostream& operator<<(std::ostream& out, Pose const& p);
140 /*! \brief Store car size.
142 * - Default is https://en.wikipedia.org/wiki/Fiat_Punto
146 double curb_to_curb = 10.820;
147 double width = 1.625;
148 double wheelbase = 2.450;
149 double distance_to_front = 3.105;
150 double length = 3.760;
152 /*! Get curb-to-curb distance. */
155 /*! Set curb-to-curb distance. */
156 void ctc(double ctc);
158 /*! Get wheelbase. */
161 /*! Set wheelbase. */
174 void len(double len);
176 /*! Get distance from rear axle to front. */
179 /*! Set distance from rear axle to front. */
182 /*! Get distance from rear axle to rear. */
185 /*! \brief Get minimum turning radius.
187 * Please, note that the method returns really _minimum turning radius_,
188 * which is the distance from the reare axle center to the center of
189 * left or right rotation given by the kinematics constrants, i.e.
190 * _wheelbase_ and _curb-to-curb_ distance.
192 * Sometimes _minimum turning radius_ is not radius, not minimum, or not
193 * turning. In this method, _minimum turning radius_ is minimum turning
199 /*! Store car motion. */
218 /*! \brief Geometrical computations of a bicycle car.
220 * - `x()` and `y()` methods returns coordinates of rear axle center.
222 class BicycleCar : public Pose, public CarSize, public CarMove {
225 /*! \brief Return `false` if `bc` is not achievable.
227 * When `false` is returned the `bc` may still be drivable, but not
228 * trivially, i.e. by "line segment - circle arc - line segment".
230 * \param bc The bicycle car to achieve.
231 * \param b The beginning of the heading range.
232 * \param e The end of the heading range.
234 bool drivable(Pose const& p, double b, double e) const;
235 bool drivable(Pose const& p) const;
237 /*! \brief Return inner radius.
239 * The inner radius is the distance from minimum turning radius circle
240 * center to the nearest point on the car. In this case, the nearest
241 * points on the car are rear axle endpoints.
243 double iradi() const;
245 /*! \brief Return outer front radius.
247 * The outer front radius is the distance from minimum turning radius
248 * circle center to the farthest point on the front (from the rear axle
249 * view) part of the car.
251 double ofradi() const;
253 /*! \brief Return outer rear radius.
255 * The outer rear radius is the distance from minimum turning radius
256 * circle center to the farthest point on the rear (from the rear axle
257 * view) part of the car.
259 double orradi() const;
261 /*! \brief Return length of perfect parking slot.
263 * The width of the slot is the same as the width of the car.
265 * \see Simon R. Blackburn *The Geometry of Perfect Parking*
266 * \see https://www.ma.rhul.ac.uk/SRBparking
268 double perfect_parking_slot_len() const;
270 /*! Set maximum steering angle. */
271 void set_max_steer();
273 /*! Get frame's left front x coordinate. */
276 /*! Get frame's left front y coordinate. */
279 /*! Get frame's left rear x coordinate. */
282 /*! Get frame's left rear y coordinate. */
285 /*! Get frame's right rear x coordinate. */
288 /*! Get frame's right rear y coordinate. */
291 /*! Get frame's right front x coordinate. */
294 /*! Get frame's right front y coordinate. */
297 /*! Get rear axle's left x coordinate. */
300 /*! Get rear axle's left y coordinate. */
303 /*! Get rear axle's right x coordinate. */
306 /*! Get rear axle's right y coordinate. */
309 /*! Min. turning radius circle center on left. */
312 /*! Min. turning radius circle center on rigth. */
315 /*! Next car position based on speed `sp` and steer `st`. */
320 #endif /* BCAR_BCAR_H */