10 /*! \brief Bicycle car basic class.
12 This class contains some geometrical computations of bicycle car.
14 \param x Horizontal coordinate of rear axle center.
15 \param y Vertical coordinate of rear axle center.
16 \param h Heading of the car in the interval [-pi,+pi] radians.
17 \param mtr Minimum turning radius.
19 \param w The width of the car.
20 \param l The length of the car.
21 \param he The height of the car.
22 \param sd The safety distance.
23 \param df Distance from rear axle center to the front of the car.
24 \param dr Distance from rear axle center to the back of the car.
25 \param sp Speed of the car.
26 \param st Steering of the car.
34 // kinematic constraints
35 double ctc_ = 10.820; // curb-to-curb
36 // FIXME is not mtr; curb-to-curb is 10.820
50 // kinematic constraints
51 /*! \brief Return `false` if `bc` is not achievable.
53 When `false` is returned the `bc` may still be drivable,
54 because only "line segment - circle arc - line segment"
55 paths are considered in ``drivable`` method.
57 \param[in] bc The bicycle car to achieve.
59 bool drivable(const BicycleCar &bc) const;
60 bool drivable(const BicycleCar &bc, double b, double e) const;
61 /*! \brief Return inner radius.
63 The inner radius is the distance from minimum turning
64 radius circle center to the nearest point on the car. In
65 this case, the nearest points on the car are rear axle
69 /*! \brief Return outer front radius.
71 The outer front radius is the distance from minimum
72 turning radius circle center to the farthest point on
73 the front (from the rear axle view) part of the car.
75 double ofradi() const;
76 /*! \brief Return outer rear radius.
78 The outer rear radius is the distance from minimum
79 turning radius circle center to the farthest point on
80 the rear (from the rear axle view) part of the car.
82 double orradi() const;
83 /*! \brief Return length of perfect parking slot.
85 The width of the slot is the same as the width of the
88 double perfect_parking_slot_len() const;
89 /*! \brief Set maximum steering angle.
94 double lfx() const; double lfy() const;
95 double lrx() const; double lry() const;
96 double rrx() const; double rry() const;
97 double rfx() const; double rfy() const;
99 double ralx() const; double raly() const;
100 double rarx() const; double rary() const;
102 /*! \brief Min. turning radius circle center on left.
104 Important are coordinates `x` and `y`. The heading `h`
105 is set as the heading of `this->h()`.
107 BicycleCar ccl() const;
108 /*! \brief Min. turning radius circle center on rigth.
110 Important are coordinates `x` and `y`. The heading `h`
111 is set as the heading of `this->h()`.
113 BicycleCar ccr() const;
116 /*! \brief Next car position based on `sp` and `st`.
118 Where `sp` is speed and `st` is steering of the car.
121 /*! \brief Rotate self around the point.
123 \param cx Horizontal coordinate of rotation center.
124 \param cy Vertical coordinate of rotation center.
125 \param angl Angle of rotation.
127 void rotate(double cx, double cy, double angl);
130 double x() const { return this->x_; }
131 void x(double x) { this->x_ = x; }
133 double y() const { return this->y_; }
134 void y(double y) { this->y_ = y; }
136 double h() const { return this->h_; }
146 double ctc() const { return this->ctc_; }
147 void ctc(double ctc) { this->ctc_ = ctc; }
149 double mtr() const { return this->mtr_; }
150 void mtr(double mtr) { this->mtr_ = mtr; }
152 double wb() const { return this->wb_; }
153 void wb(double wb) { this->wb_ = wb; }
155 double w() const { return this->w_; }
156 void w(double w) { this->w_ = w; }
158 double l() const { return this->l_; }
159 void l(double l) { this->l_ = l; }
161 double he() const { return this->he_; }
162 void he(double he) { this->he_ = he; }
164 double sd() const { return this->sd_; }
165 void sd(double sd) { this->sd_ = sd; }
167 double df() const { return this->df_; }
168 void df(double df) { this->df_ = df; }
170 double dr() const { return this->dr_; }
171 void dr(double dr) { this->dr_ = dr; }
173 double sp() const { return this->sp_; }
174 void sp(double sp) { this->sp_ = sp; }
176 double st() const { return this->st_; }
177 void st(double st) { this->st_ = st; }
180 friend std::ostream &operator<<(
185 out << "[" << bc.x();
186 out << "," << bc.y();
187 out << "," << bc.h();
193 /*! \brief Does two polygons collide?
195 Return the tuple `std::tuple<bool, int, int>`, where the first value is
196 `true` when there is an intersection of some segments of the polygons
197 `p1` and `p2` and `false` otherwise. The second and third parameters in
198 the return tuple are indexes of the first collision, where index starts
201 \param p1 The first polygon to check against collision.
202 \param p2 The second polygon to check against collision.
204 std::tuple<bool, unsigned int, unsigned int>
206 std::vector<std::tuple<double, double>> &p1,
207 std::vector<std::tuple<double, double>> &p2
210 /*! \brief Is `x, y` coordinate in polygon `poly`?
212 Return `true` if `x, y` coordinate is inside of polygon `poly`.
214 \see https://en.wikipedia.org/wiki/Even%E2%80%93odd_rule
216 \param x Horizontal coordinate.
217 \param y Vertical coordinate.
218 \param poly The vector of coordinates.
221 inside(double x, double y, std::vector<std::tuple<double, double>> &poly);
223 /*! \brief Return intersection of two line segments.
225 The output is tuple `std::tuple<bool, double, double>`, where the first
226 value is true when there is an intersection and false otherwise. The
227 second and third parameters in the return tuple are coordinates of the
230 \see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
232 \param x1 First line segment first `x` coordinate.
233 \param y1 First line segment first `y` coordinate.
234 \param x2 First line segment second `x` coordinate.
235 \param y2 First line segment second `y` coordinate.
236 \param x3 Second line segment first `x` coordinate.
237 \param y3 Second line segment first `y` coordinate.
238 \param x4 Second line segment second `x` coordinate.
239 \param y4 Second line segment second `y` coordinate.
241 std::tuple<bool, double, double>
243 double x1, double y1,
244 double x2, double y2,
245 double x3, double y3,
249 /*! \brief Return intersections of (infinite) line and circle.
251 The output is tuple `std::tuble<bool, double, double, double, double>`, where
252 the first value is true when there is an intersection and false otherwise. The
253 second and third parameters in the return tuple are coordinates of the first
254 intersection. The fourth and fifth parameters in the return tuple are
255 coordinates of the second intersection.
257 \see https://mathworld.wolfram.com/Circle-LineIntersection.html
259 \param cx Circle center `x` coordinate.
260 \param cy Circle center `y` coordinate.
261 \param r Circle radius.
262 \param x1 Line segment first `x` coordinate.
263 \param y1 Line segment first `y` coordinate.
264 \param x2 Line segment second `x` coordinate.
265 \param y2 Line segment second `y` coordinate.
267 std::tuple<bool, double, double, double, double>
269 double cx, double cy, double r,
270 double x1, double y1,
274 /*! \brief Return the smallest angle between three points.
276 \see https://math.stackexchange.com/questions/361412/finding-the-angle-between-three-points
286 angle_between_three_points(
287 double x1, double y1,
288 double x2, double y2,
292 /*! \brief Return if point is on the right side of plane.
294 \param x1 Line first `x` coordinate.
295 \param y1 Line first `y` coordinate.
296 \param x2 Line second `x` coordinate.
297 \param y2 Line second `y` coordinate.
298 \param x3 Point to decide `x` coordinate.
299 \param y3 Point to decide `y` coordinate.
303 double x1, double y1,
304 double x2, double y2,