4 // kinematic constraints
5 bool BicycleCar::drivable(const BicycleCar &bc) const
7 return this->drivable(bc, bc.h(), bc.h());
9 bool BicycleCar::drivable(const BicycleCar &bc, double b, double e) const
11 // assert bc.h() == (b + e) / 2.0
12 double a_1 = atan2(bc.y() - this->y(), bc.x() - this->x()) - this->h();
17 double h_d = bc.h() - this->h();
23 if (h_d == 0 && (a_1 == 0 || a_2 == M_PI || a_2 == -M_PI)) {
25 } else if (0 < a_1 && a_1 <= M_PI/2) { // left front
26 BicycleCar z(*this); // zone border
28 h_d = bc.h() - this->h();
29 z.rotate(this->ccl().x(), this->ccl().y(), h_d);
30 // assert z.h() == bc.h()
31 if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
33 a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
38 if (z.h() >= a_2 && a_2 >= this->h())
40 } else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear
41 BicycleCar z(*this); // zone border
43 h_d = bc.h() - this->h();
44 z.rotate(this->ccl().x(), this->ccl().y(), h_d);
45 // assert z.h() == bc.h()
46 if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
48 a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
54 if (this->h() >= a_2 && a_2 >= z.h())
56 } else if (0 > a_1 && a_1 >= -M_PI/2) { // right front
57 BicycleCar z(*this); // zone border
59 h_d = bc.h() - this->h();
60 z.rotate(this->ccr().x(), this->ccr().y(), h_d);
61 // assert z.h() == bc.h()
62 if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
64 a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
69 if (this->h() >= a_2 && a_2 >= z.h())
71 } else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear
72 BicycleCar z(*this); // zone border
74 h_d = bc.h() - this->h();
75 z.rotate(this->ccr().x(), this->ccr().y(), h_d);
76 // assert z.h() == bc.h()
77 if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
79 a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
85 if (z.h() >= a_2 && a_2 >= this->h())
88 // Not happenning, as ``-pi <= a <= pi``.
93 double BicycleCar::iradi() const
95 return this->mtr() - this->w() / 2;
98 double BicycleCar::ofradi() const
100 return sqrt(pow(this->mtr() + this->w() / 2, 2) + pow(this->df(), 2));
103 double BicycleCar::orradi() const
105 return sqrt(pow(this->mtr() + this->w() / 2, 2) + pow(this->dr(), 2));
108 double BicycleCar::perfect_parking_slot_len() const
110 // see Simon R. Blackburn *The Geometry of Perfect Parking*
111 // see https://www.ma.rhul.ac.uk/SRBparking
112 double r = this->ctc() / 2;
113 double l = this->wb();
114 double k = this->df() - this->wb();
115 double w = this->w();
121 - pow(sqrt(r*r - l*l) - w, 2)
128 void BicycleCar::set_max_steer()
130 this->st(atan(this->wb() / this->mtr()));
134 double BicycleCar::lfx() const
136 double lfx = this->x();
137 lfx += (this->w() / 2) * cos(this->h() + M_PI / 2);
138 lfx += this->df() * cos(this->h());
139 lfx += this->sd() * cos(this->h());
143 double BicycleCar::lfy() const
145 double lfy = this->y();
146 lfy += (this->w() / 2) * sin(this->h() + M_PI / 2);
147 lfy += this->df() * sin(this->h());
148 lfy += this->sd() * sin(this->h());
152 double BicycleCar::lrx() const
154 double lrx = this->x();
155 lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
156 lrx += -this->dr() * cos(this->h());
157 lrx += -this->sd() * cos(this->h());
161 double BicycleCar::lry() const
163 double lry = this->y();
164 lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
165 lry += -this->dr() * sin(this->h());
166 lry += -this->sd() * sin(this->h());
170 double BicycleCar::rrx() const
172 double rrx = this->x();
173 rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
174 rrx += -this->dr() * cos(this->h());
175 rrx += -this->sd() * cos(this->h());
179 double BicycleCar::rry() const
181 double rry = this->y();
182 rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
183 rry += -this->dr() * sin(this->h());
184 rry += -this->sd() * sin(this->h());
188 double BicycleCar::rfx() const
190 double rfx = this->x();
191 rfx += (this->w() / 2) * cos(this->h() - M_PI / 2);
192 rfx += this->df() * cos(this->h());
193 rfx += this->sd() * cos(this->h());
197 double BicycleCar::rfy() const
199 double rfy = this->y();
200 rfy += (this->w() / 2) * sin(this->h() - M_PI / 2);
201 rfy += this->df() * sin(this->h());
202 rfy += this->sd() * sin(this->h());
206 double BicycleCar::ralx() const
208 double lrx = this->x();
209 lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
212 double BicycleCar::raly() const
214 double lry = this->y();
215 lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
219 double BicycleCar::rarx() const
221 double rrx = this->x();
222 rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
226 double BicycleCar::rary() const
228 double rry = this->y();
229 rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
233 BicycleCar BicycleCar::ccl() const
236 bc.x(this->x() + this->mtr() * cos(this->h() + M_PI / 2));
237 bc.y(this->y() + this->mtr() * sin(this->h() + M_PI / 2));
242 BicycleCar BicycleCar::ccr() const
245 bc.x(this->x() + this->mtr() * cos(this->h() - M_PI / 2));
246 bc.y(this->y() + this->mtr() * sin(this->h() - M_PI / 2));
252 void BicycleCar::next()
254 this->x(this->x() + this->sp() * cos(this->h()));
255 this->y(this->y() + this->sp() * sin(this->h()));
256 this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
259 void BicycleCar::rotate(double cx, double cy, double angl)
261 double px = this->x();
262 double py = this->y();
265 double nx = px * cos(angl) - py * sin(angl);
266 double ny = px * sin(angl) + py * cos(angl);
267 this->h(this->h() + angl);
272 BicycleCar::BicycleCar()
274 // TODO according to mtr_ FIXME
283 std::tuple<bool, unsigned int, unsigned int>
285 std::vector<std::tuple<double, double>> &p1,
286 std::vector<std::tuple<double, double>> &p2
289 for (unsigned int i = 0; i < p1.size() - 1; i++) {
290 for (unsigned int j = 0; j < p2.size() - 1; j++) {
294 std::get<0>(p1[i + 1]),
295 std::get<1>(p1[i + 1]),
298 std::get<0>(p2[j + 1]),
299 std::get<1>(p2[j + 1])
302 return std::make_tuple(true, i, j);
305 return std::make_tuple(false, 0, 0);
309 inside(double x, double y, std::vector<std::tuple<double, double>> &poly)
314 for (i = 0; i < 4; i++) {
316 (std::get<1>(poly[i]) > y) != (std::get<1>(poly[j]) > y)
318 x < std::get<0>(poly[i])
319 + (std::get<0>(poly[j]) - std::get<0>(poly[i]))
320 * (y - std::get<1>(poly[i]))
321 / (std::get<1>(poly[j]) - std::get<1>(poly[i]))
330 std::tuple<bool, double, double>
332 double x1, double y1,
333 double x2, double y2,
334 double x3, double y3,
338 double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
340 return std::make_tuple(false, 0, 0);
341 double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
343 double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
346 if (t < 0 || t > 1 || u < 0 || u > 1)
347 return std::make_tuple(false, 0, 0);
348 return std::make_tuple(true, x1 + t * (x2 - x1), y1 + t * (y2 - y1));
351 std::tuple<bool, double, double, double, double>
353 double cx, double cy, double r,
354 double x1, double y1,
365 double dr = sqrt(dx*dx + dy*dy);
366 double D = x1*y2 - x2*y1;
367 if (r*r * dr*dr - D*D < 0)
368 return std::make_tuple(false, 0, 0, 0, 0);
369 // intersection coordinates
370 double ix1 = (D*dy + sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
372 double ix2 = (D*dy - sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
374 double iy1 = (-D*dx + std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
376 double iy2 = (-D*dx - std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
378 return std::make_tuple(true, ix1, iy1, ix2, iy2);
382 angle_between_three_points(
383 double x1, double y1,
384 double x2, double y2,
387 double d1x = x2 - x1;
388 double d1y = y2 - y1;
389 double d2x = x3 - x2;
390 double d2y = y3 - y2;
392 double dot = d1x*d2x + d1y*d2y;
393 double d1 = sqrt(d1x*d1x + d1y*d1y);
394 double d2 = sqrt(d2x*d2x + d2y*d2y);
396 double delta = acos(dot / (d1 * d2));
397 return std::min(delta, M_PI - delta);
402 double x1, double y1,
403 double x2, double y2,
406 if (sgn((x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1)) < 0)