src/bcar.cc

   1 /*
   2  * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 #include <cmath>
   8 #include "bcar.hh"
   9
  10 namespace bcar {
  11
  12 Point::Point()
  13 {
  14 }
  15
  16 Point::Point(double x, double y) : _x(x), _y(y)
  17 {
  18 }
  19
  20 double
  21 Point::x() const
  22 {
  23         return this->_x;
  24 }
  25
  26 void
  27 Point::x(double x)
  28 {
  29         this->_x = x;
  30 }
  31
  32 double
  33 Point::y() const
  34 {
  35         return this->_y;
  36 }
  37
  38 void
  39 Point::y(double y)
  40 {
  41         this->_y = y;
  42 }
  43
  44 double
  45 Point::min_angle_between(Point const& p1, Point const& p2) const
  46 {
  47         double d1x = p1.x() - this->x();
  48         double d1y = p1.y() - this->y();
  49         double d2x = p2.x() - p1.x();
  50         double d2y = p2.y() - p1.y();
  51
  52         double dot = d1x*d2x + d1y*d2y;
  53         double d1 = sqrt(d1x*d1x + d1y*d1y);
  54         double d2 = sqrt(d2x*d2x + d2y*d2y);
  55
  56         double delta = acos(dot / (d1 * d2));
  57         return std::min(delta, M_PI - delta);
  58 }
  59
  60 bool
  61 Point::inside_of(std::vector<Point> const& poly) const
  62 {
  63         unsigned int num = poly.size();
  64         unsigned int j = num - 1;
  65         bool c = false;
  66         for (unsigned int i = 0; i < num; i++) {
  67                 if (this->x() == poly[i].x() && this->y() == poly[i].y()) {
  68                         return true;
  69                 }
  70                 if ((poly[i].y() > this->y()) != (poly[j].y() > this->y())) {
  71                         auto slope1 = this->x() - poly[i].x();
  72                         slope1 *= poly[j].y() - poly[i].y();
  73                         auto slope2 = poly[j].x() - poly[i].x();
  74                         slope2 *= this->y() - poly[i].y();
  75                         auto slope = slope1 - slope2;
  76                         if (slope == 0.0) {
  77                                 return true;
  78                         }
  79                         if ((slope < 0.0) != (poly[j].y() < poly[i].y())) {
  80                                 c = !c;
  81                         }
  82                 }
  83                 j = i;
  84         }
  85         return c;
  86 }
  87
  88 bool
  89 Point::on_right_side_of(Line const& li) const
  90 {
  91         auto x1 = li.b().x();
  92         auto y1 = li.b().y();
  93         auto x2 = li.e().x();
  94         auto y2 = li.e().y();
  95         auto x3 = this->_x;
  96         auto y3 = this->_y;
  97         if (sgn((x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1)) < 0.0) {
  98                 return false;
  99         } else {
 100                 return true;
 101         }
 102 }
 103
 104 void
 105 Point::translate(Point const& p)
 106 {
 107         this->_x += p.x();
 108         this->_y += p.y();
 109 }
 110
 111 void
 112 Point::rotate(Point const& c, double const angl)
 113 {
 114         double px = this->x();
 115         double py = this->y();
 116         px -= c.x();
 117         py -= c.y();
 118         double nx = px * cos(angl) - py * sin(angl);
 119         double ny = px * sin(angl) + py * cos(angl);
 120         this->x(nx + c.x());
 121         this->y(ny + c.y());
 122 }
 123
 124 void
 125 Point::reflect(Line const& li)
 126 {
 127         this->rotate(li.b(), -li.h());
 128         this->_y -= li.b().y();
 129         this->_y *= -1.0;
 130         this->_y += li.b().y();
 131         this->rotate(li.b(), li.h());
 132 }
 133
 134 double
 135 Point::edist(Point const& p) const
 136 {
 137         return sqrt(pow(p.x() - this->_x, 2.0) + pow(p.y() - this->_y, 2.0));
 138 }
 139
 140 void
 141 Point::gen_gnuplot_to(std::ostream& out)
 142 {
 143         out << this->_x << " " << this->_y << std::endl;
 144 }
 145
 146 bool
 147 Point::operator==(Point const& p)
 148 {
 149         return this->x() == p.x() && this->y() == p.y();
 150 }
 151
 152 std::ostream&
 153 operator<<(std::ostream& out, Point const& p)
 154 {
 155         out << "[" << p.x() << "," << p.y() << "]";
 156         return out;
 157 }
 158
 159 Line::Line(Point const& b, Point const& e): _b(b), _e(e)
 160 {
 161 }
 162
 163 Point
 164 Line::b() const&
 165 {
 166         return this->_b;
 167 }
 168
 169 Point
 170 Line::e() const&
 171 {
 172         return this->_e;
 173 }
 174
 175 Point
 176 Line::m() const
 177 {
 178         return Point((this->_b.x() + this->_e.x()) / 2.0,
 179                 (this->_b.y() + this->_e.y()) / 2.0);
 180 }
 181
 182 Point
 183 Line::i1() const&
 184 {
 185         return this->_i1;
 186 }
 187
 188 Point
 189 Line::i2() const&
 190 {
 191         return this->_i2;
 192 }
 193
 194 bool
 195 Line::intersects_with(Line const& li)
 196 {
 197         auto x1 = this->_b.x();
 198         auto y1 = this->_b.y();
 199         auto x2 = this->_e.x();
 200         auto y2 = this->_e.y();
 201         auto x3 = li.b().x();
 202         auto y3 = li.b().y();
 203         auto x4 = li.e().x();
 204         auto y4 = li.e().y();
 205         double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
 206         if (deno == 0.0) {
 207                 return false;
 208         }
 209         double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
 210         t /= deno;
 211         double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
 212         u *= -1.0;
 213         u /= deno;
 214         if (t < 0.0 || t > 1.0 || u < 0.0 || u > 1.0) {
 215                 return false;
 216         }
 217         this->_i1.x(x1 + t * (x2 - x1));
 218         this->_i1.y(y1 + t * (y2 - y1));
 219         return true;
 220 }
 221
 222 bool
 223 Line::intersects_with(Point const& c, double const r)
 224 {
 225         auto x1 = this->_b.x();
 226         auto y1 = this->_b.y();
 227         auto x2 = this->_e.x();
 228         auto y2 = this->_e.y();
 229         auto cx = c.x();
 230         auto cy = c.y();
 231         x2 -= cx;
 232         x1 -= cx;
 233         y2 -= cy;
 234         y1 -= cy;
 235         if (y1 == y2) {
 236                 y1 += 0.00001;
 237         }
 238         double dx = x2 - x1;
 239         double dy = y2 - y1;
 240         double dr = sqrt(dx*dx + dy*dy);
 241         double D = x1*y2 - x2*y1;
 242         if (r*r * dr*dr - D*D < 0.0) {
 243                 return false;
 244         }
 245         // intersection coordinates
 246         double ix1 = (D*dy + sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
 247         ix1 += cx;
 248         double ix2 = (D*dy - sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
 249         ix2 += cx;
 250         double iy1 = (-D*dx + std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
 251         iy1 += cy;
 252         double iy2 = (-D*dx - std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
 253         iy2 += cy;
 254         this->_i1.x(ix1);
 255         this->_i1.y(iy1);
 256         this->_i2.x(ix2);
 257         this->_i2.y(iy2);
 258         return true;
 259 }
 260
 261 double
 262 Line::len() const
 263 {
 264         return this->_b.edist(this->_e);
 265 }
 266
 267 double
 268 Line::h() const
 269 {
 270         return atan2(this->_e.y() - this->_b.y(), this->_e.x() - this->_b.x());
 271 }
 272
 273 void
 274 Line::gen_gnuplot_to(std::ostream& out)
 275 {
 276         this->b().gen_gnuplot_to(out);
 277         this->e().gen_gnuplot_to(out);
 278         out << std::endl;
 279 }
 280
 281 std::ostream&
 282 operator<<(std::ostream& out, Line const& li)
 283 {
 284         out << "[" << li._b << "," << li._e << "]";
 285         return out;
 286 }
 287
 288 Pose::Pose(double x, double y, double h) : Point(x, y), _h(h)
 289 {
 290 }
 291
 292 double
 293 Pose::h() const
 294 {
 295         return this->_h;
 296 }
 297
 298 void
 299 Pose::h(double h)
 300 {
 301         while (h < -M_PI) {
 302                 h += 2 * M_PI;
 303         }
 304         while (h > +M_PI) {
 305                 h -= 2 * M_PI;
 306         }
 307         this->_h = h;
 308 }
 309
 310 void
 311 Pose::set_pose(Pose const& p)
 312 {
 313         this->x(p.x());
 314         this->y(p.y());
 315         this->h(p.h());
 316 }
 317
 318 void
 319 Pose::rotate(Point const& c, double const angl)
 320 {
 321         Point::rotate(c, angl);
 322         this->h(this->h() + angl);
 323 }
 324
 325 void
 326 Pose::reflect(Line const& li)
 327 {
 328         Point::reflect(li);
 329         double dh = li.h() - this->h();
 330         this->h(this->h() + 2.0 * dh);
 331 }
 332
 333 bool
 334 Pose::operator==(Pose const& p)
 335 {
 336         return this->x() == p.x() && this->y() == p.y() && this->h() == p.h();
 337 }
 338
 339 std::ostream&
 340 operator<<(std::ostream& out, Pose const& p)
 341 {
 342         out << "[" << p.x() << "," << p.y() << "," << p.h() << "]";
 343         return out;
 344 }
 345
 346 void
 347 PoseRange::set_xyh()
 348 {
 349         double clen = 10.0;
 350         double bpbx = this->_bp.x() - clen * cos(this->_bp.h());
 351         double bpby = this->_bp.y() - clen * sin(this->_bp.h());
 352         double bpfx = this->_bp.x() + clen * cos(this->_bp.h());
 353         double bpfy = this->_bp.y() + clen * sin(this->_bp.h());
 354         Line li1(Point(bpbx, bpby), Point(bpfx, bpfy));
 355         double epbx = this->_ep.x() - clen * cos(this->_ep.h());
 356         double epby = this->_ep.y() - clen * sin(this->_ep.h());
 357         double epfx = this->_ep.x() + clen * cos(this->_ep.h());
 358         double epfy = this->_ep.y() + clen * sin(this->_ep.h());
 359         Line li2(Point(epbx, epby), Point(epfx, epfy));
 360         li1.intersects_with(li2);
 361         this->x(li1.i1().x());
 362         this->y(li1.i1().y());
 363         double bh = this->b();
 364         while (bh < 0.0) {
 365                 bh += 2.0 * M_PI;
 366         }
 367         this->_bp.h(bh);
 368         double eh = this->e();
 369         while (eh < 0.0) {
 370                 eh += 2.0 * M_PI;
 371         }
 372         this->_ep.h(eh);
 373         this->h((this->b() + this->e()) / 2.0);
 374 }
 375
 376 PoseRange::PoseRange(Pose bp, Pose ep) : _bp(bp), _ep(ep)
 377 {
 378         if (this->_bp == this->_ep) {
 379                 this->set_pose(this->_ep);
 380         } else {
 381                 this->set_xyh();
 382         }
 383 }
 384
 385 PoseRange::PoseRange(double x, double y, double b, double e)
 386                 : PoseRange(Pose(x, y, b), Pose(x, y, e))
 387 {
 388 }
 389
 390 Pose
 391 PoseRange::bp() const
 392 {
 393         return this->_bp;
 394 }
 395
 396 Pose
 397 PoseRange::ep() const
 398 {
 399         return this->_ep;
 400 }
 401
 402 double
 403 PoseRange::b() const
 404 {
 405         return std::min(this->_bp.h(), this->_ep.h());
 406 }
 407
 408 double
 409 PoseRange::e() const
 410 {
 411         return std::max(this->_bp.h(), this->_ep.h());
 412 }
 413
 414 void
 415 PoseRange::translate(Point const& p)
 416 {
 417         this->_bp.translate(p);
 418         this->_ep.translate(p);
 419         this->set_xyh();
 420 }
 421
 422 void
 423 PoseRange::rotate(Point const& c, double const angl)
 424 {
 425         this->_bp.rotate(c, angl);
 426         this->_ep.rotate(c, angl);
 427         this->set_xyh();
 428 }
 429
 430 void
 431 PoseRange::reflect(Line const& li)
 432 {
 433         this->_bp.reflect(li);
 434         this->_ep.reflect(li);
 435         this->set_xyh();
 436 }
 437
 438 std::ostream&
 439 operator<<(std::ostream& out, PoseRange const& p)
 440 {
 441         out << "[" << p.x() << "," << p.y() << "," << p.b() << "," << p.e();
 442         out << "]";
 443         return out;
 444 }
 445
 446 double
 447 CarSize::ctc() const
 448 {
 449         return this->_curb_to_curb;
 450 }
 451
 452 void
 453 CarSize::ctc(double ctc)
 454 {
 455         this->_curb_to_curb = ctc;
 456 }
 457
 458 double
 459 CarSize::wb() const
 460 {
 461         return this->_wheelbase;
 462 }
 463
 464 void
 465 CarSize::wb(double wb)
 466 {
 467         this->_wheelbase = wb;
 468 }
 469
 470 double
 471 CarSize::w() const
 472 {
 473         return this->_width;
 474 }
 475
 476 void
 477 CarSize::w(double w)
 478 {
 479         this->_width = w;
 480 }
 481
 482 double
 483 CarSize::wwm() const
 484 {
 485         return this->_width_with_mirrors;
 486 }
 487
 488 void
 489 CarSize::wwm(double w)
 490 {
 491         this->_width_with_mirrors = w;
 492 }
 493
 494 double
 495 CarSize::len() const
 496 {
 497         return this->_length;
 498 }
 499
 500 void
 501 CarSize::len(double len)
 502 {
 503         this->_length = len;
 504 }
 505
 506 double
 507 CarSize::df() const
 508 {
 509         return this->_distance_to_front;
 510 }
 511
 512 void
 513 CarSize::df(double df)
 514 {
 515         this->_distance_to_front = df;
 516 }
 517
 518 double
 519 CarSize::dr() const
 520 {
 521         return this->len() - this->df();
 522 }
 523
 524 void
 525 CarSize::ft(double ft)
 526 {
 527         this->_front_track = ft;
 528 }
 529
 530 double
 531 CarSize::ft() const
 532 {
 533         return this->_front_track;
 534 }
 535
 536 double
 537 CarSize::mtr() const
 538 {
 539         auto ctc2 = pow(this->ctc() / 2.0, 2.0);
 540         auto wb2 = pow(this->wb(), 2.0);
 541         return sqrt(ctc2 - wb2) - this->ft() / 2.0;
 542 }
 543
 544 double
 545 CarSize::iradi() const
 546 {
 547         return this->mtr() - this->w() / 2;
 548 }
 549
 550 double
 551 CarSize::ofradi() const
 552 {
 553         auto mtrw2 = pow(this->mtr() + this->w() / 2.0, 2.0);
 554         auto df2 = pow(this->df(), 2.0);
 555         return sqrt(mtrw2 + df2);
 556 }
 557
 558 double
 559 CarSize::orradi() const
 560 {
 561         auto mtrw2 = pow(this->mtr() + this->w() / 2.0, 2.0);
 562         auto dr2 = pow(this->dr(), 2.0);
 563         return sqrt(mtrw2 + dr2);
 564 }
 565
 566 double
 567 CarSize::imradi() const
 568 {
 569         auto mtrw2 = pow(this->mtr() - this->wwm() / 2.0, 2.0);
 570         auto df2 = pow(this->wb(), 2.0);
 571         return sqrt(mtrw2 + df2);
 572 }
 573
 574 double
 575 CarSize::omradi() const
 576 {
 577         auto mtrw2 = pow(this->mtr() + this->wwm() / 2.0, 2.0);
 578         auto df2 = pow(this->wb(), 2.0);
 579         return sqrt(mtrw2 + df2);
 580 }
 581
 582 double
 583 CarSize::perfect_parking_slot_len() const
 584 {
 585         auto r = this->ctc() / 2.0;
 586         auto l = this->wb();
 587         auto k = this->df() - this->wb();
 588         auto w = this->w(); // FIXME use wwm()?
 589         auto r2l2 = r * r - l * l;
 590         auto s = r2l2 + pow(l + k, 2.0) - pow(sqrt(r2l2) - w, 2.0);
 591         return this->len() + sqrt(s) - l - k;
 592 }
 593
 594 double
 595 CarMove::sp() const
 596 {
 597         return this->_speed;
 598 }
 599
 600 void
 601 CarMove::sp(double sp)
 602 {
 603         this->_speed = sp;
 604 }
 605
 606 double
 607 CarMove::st() const
 608 {
 609         return this->_steer;
 610 }
 611
 612 void
 613 CarMove::st(double st)
 614 {
 615         this->_steer = st;
 616 }
 617
 618 bool
 619 BicycleCar::drivable(Pose const& p) const
 620 {
 621         return this->drivable(PoseRange(p, p));
 622 }
 623
 624 bool
 625 BicycleCar::drivable(PoseRange const& p) const
 626 {
 627         double a_1 = atan2(p.y() - this->y(), p.x() - this->x()) - this->h();
 628         while (a_1 < -M_PI)
 629                 a_1 += 2 * M_PI;
 630         while (a_1 > +M_PI)
 631                 a_1 -= 2 * M_PI;
 632         double h_d = p.h() - this->h();
 633         while (h_d < -M_PI)
 634                 h_d += 2 * M_PI;
 635         while (h_d > +M_PI)
 636                 h_d -= 2 * M_PI;
 637         double a_2 = 0;
 638         if (h_d == 0 && (a_1 == 0 || a_2 == M_PI || a_2 == -M_PI)) {
 639                 return true;
 640         } else if (0 < a_1 && a_1 <= M_PI/2) { // left front
 641                 BicycleCar z(*this); // zone border
 642                 z.h(p.e());
 643                 h_d = p.h() - this->h();
 644                 z.rotate(this->ccl(), h_d);
 645                 // assert z.h() == p.h()
 646                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 647                         return true;
 648                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 649                 while (a_2 < -M_PI)
 650                         a_2 += 2 * M_PI;
 651                 while (a_2 > +M_PI)
 652                         a_2 -= 2 * M_PI;
 653                 if (z.h() >= a_2 && a_2 >= this->h())
 654                         return true;
 655         } else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear
 656                 BicycleCar z(*this); // zone border
 657                 z.h(p.e());
 658                 h_d = p.h() - this->h();
 659                 z.rotate(this->ccl(), h_d);
 660                 // assert z.h() == p.h()
 661                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 662                         return true;
 663                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 664                 a_2 -= M_PI;
 665                 while (a_2 < -M_PI)
 666                         a_2 += 2 * M_PI;
 667                 while (a_2 > +M_PI)
 668                         a_2 -= 2 * M_PI;
 669                 if (this->h() >= a_2 && a_2 >= z.h())
 670                         return true;
 671         } else if (0 > a_1 && a_1 >= -M_PI/2) { // right front
 672                 BicycleCar z(*this); // zone border
 673                 z.h(p.b());
 674                 h_d = p.h() - this->h();
 675                 z.rotate(this->ccr(), h_d);
 676                 // assert z.h() == p.h()
 677                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 678                         return true;
 679                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 680                 while (a_2 < -M_PI)
 681                         a_2 += 2 * M_PI;
 682                 while (a_2 > +M_PI)
 683                         a_2 -= 2 * M_PI;
 684                 if (this->h() >= a_2 && a_2 >= z.h())
 685                         return true;
 686         } else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear
 687                 BicycleCar z(*this); // zone border
 688                 z.h(p.b());
 689                 h_d = p.h() - this->h();
 690                 z.rotate(this->ccr(), h_d);
 691                 // assert z.h() == p.h()
 692                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 693                         return true;
 694                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 695                 a_2 -= M_PI;
 696                 while (a_2 < -M_PI)
 697                         a_2 += 2 * M_PI;
 698                 while (a_2 > +M_PI)
 699                         a_2 -= 2 * M_PI;
 700                 if (z.h() >= a_2 && a_2 >= this->h())
 701                         return true;
 702         } else {
 703                 // Not happenning, as ``-pi <= a <= pi``.
 704         }
 705         return false;
 706 }
 707
 708 void
 709 BicycleCar::set_max_steer()
 710 {
 711         this->st(atan(this->wb() / this->mtr()));
 712 }
 713
 714 double
 715 BicycleCar::lfx() const
 716 {
 717         double lfx = this->x();
 718         lfx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0);
 719         lfx += this->df() * cos(this->h());
 720         return lfx;
 721 }
 722
 723 double
 724 BicycleCar::lfy() const
 725 {
 726         double lfy = this->y();
 727         lfy += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0);
 728         lfy += this->df() * sin(this->h());
 729         return lfy;
 730 }
 731
 732 double
 733 BicycleCar::lrx() const
 734 {
 735         double lrx = this->x();
 736         lrx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0);
 737         lrx += -this->dr() * cos(this->h());
 738         return lrx;
 739 }
 740
 741 double
 742 BicycleCar::lry() const
 743 {
 744         double lry = this->y();
 745         lry += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0);
 746         lry += -this->dr() * sin(this->h());
 747         return lry;
 748 }
 749
 750 double
 751 BicycleCar::rrx() const
 752 {
 753         double rrx = this->x();
 754         rrx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0);
 755         rrx += -this->dr() * cos(this->h());
 756         return rrx;
 757 }
 758
 759 double
 760 BicycleCar::rry() const
 761 {
 762         double rry = this->y();
 763         rry += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0);
 764         rry += -this->dr() * sin(this->h());
 765         return rry;
 766 }
 767
 768 double
 769 BicycleCar::rfx() const
 770 {
 771         double rfx = this->x();
 772         rfx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0);
 773         rfx += this->df() * cos(this->h());
 774         return rfx;
 775 }
 776
 777 double
 778 BicycleCar::rfy() const
 779 {
 780         double rfy = this->y();
 781         rfy += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0);
 782         rfy += this->df() * sin(this->h());
 783         return rfy;
 784 }
 785
 786 Point
 787 BicycleCar::lf() const
 788 {
 789         return Point(this->lfx(), this->lfy());
 790 }
 791
 792 Point
 793 BicycleCar::lr() const
 794 {
 795         return Point(this->lrx(), this->lry());
 796 }
 797
 798 Point
 799 BicycleCar::rr() const
 800 {
 801         return Point(this->rrx(), this->rry());
 802 }
 803
 804 Point
 805 BicycleCar::rf() const
 806 {
 807         return Point(this->rfx(), this->rfy());
 808 }
 809
 810 Line
 811 BicycleCar::left() const
 812 {
 813         return Line(this->lr(), this->lf());
 814 }
 815
 816 Line
 817 BicycleCar::rear() const
 818 {
 819         return Line(this->lr(), this->rr());
 820 }
 821
 822 Line
 823 BicycleCar::right() const
 824 {
 825         return Line(this->rr(), this->rf());
 826 }
 827
 828 Line
 829 BicycleCar::front() const
 830 {
 831         return Line(this->rf(), this->lf());
 832 }
 833
 834 double
 835 BicycleCar::lrax() const
 836 {
 837         double lrx = this->x();
 838         lrx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0);
 839         return lrx;
 840 }
 841 double
 842 BicycleCar::lray() const
 843 {
 844         double lry = this->y();
 845         lry += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0);
 846         return lry;
 847 }
 848
 849 double
 850 BicycleCar::rrax() const
 851 {
 852         double rrx = this->x();
 853         rrx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0);
 854         return rrx;
 855 }
 856
 857 double
 858 BicycleCar::rray() const
 859 {
 860         double rry = this->y();
 861         rry += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0);
 862         return rry;
 863 }
 864
 865 Point
 866 BicycleCar::lra() const
 867 {
 868         return Point(this->lrax(), this->lray());
 869 }
 870
 871 Point
 872 BicycleCar::rra() const
 873 {
 874         return Point(this->rrax(), this->rray());
 875 }
 876
 877 double
 878 BicycleCar::lfax() const
 879 {
 880         return this->lrax() + this->wb() * cos(this->h());
 881 }
 882
 883 double
 884 BicycleCar::lfay() const
 885 {
 886         return this->lray() + this->wb() * sin(this->h());
 887 }
 888
 889 double
 890 BicycleCar::rfax() const
 891 {
 892         return this->rrax() + this->wb() * cos(this->h());
 893 }
 894
 895 double
 896 BicycleCar::rfay() const
 897 {
 898         return this->rray() + this->wb() * sin(this->h());
 899 }
 900
 901 Point
 902 BicycleCar::lfa() const
 903 {
 904         return Point(this->lfax(), this->lfay());
 905 }
 906
 907 Point
 908 BicycleCar::rfa() const
 909 {
 910         return Point(this->rfax(), this->rfay());
 911 }
 912
 913 double
 914 BicycleCar::lfmx() const
 915 {
 916         double x = this->x();
 917         x += (this->wwm() / 2.0) * cos(this->h() + M_PI / 2.0);
 918         x += this->wb() * cos(this->h());
 919         return x;
 920 }
 921
 922 double
 923 BicycleCar::lfmy() const
 924 {
 925         double y = this->y();
 926         y += (this->wwm() / 2.0) * sin(this->h() + M_PI / 2.0);
 927         y += this->wb() * sin(this->h());
 928         return y;
 929 }
 930
 931 double
 932 BicycleCar::rfmx() const
 933 {
 934         double x = this->x();
 935         x += (this->wwm() / 2.0) * cos(this->h() - M_PI / 2.0);
 936         x += this->wb() * cos(this->h());
 937         return x;
 938 }
 939
 940 double
 941 BicycleCar::rfmy() const
 942 {
 943         double y = this->y();
 944         y += (this->wwm() / 2.0) * sin(this->h() - M_PI / 2.0);
 945         y += this->wb() * sin(this->h());
 946         return y;
 947 }
 948
 949 Point
 950 BicycleCar::lfm() const
 951 {
 952         return Point(this->lfmx(), this->lfmy());
 953 }
 954
 955 Point
 956 BicycleCar::rfm() const
 957 {
 958         return Point(this->rfmx(), this->rfmy());
 959 }
 960
 961 double
 962 BicycleCar::cfx() const
 963 {
 964         return this->x() + this->df() * cos(this->h());
 965 }
 966
 967 double
 968 BicycleCar::cfy() const
 969 {
 970         return this->y() + this->df() * sin(this->h());
 971 }
 972
 973 Point
 974 BicycleCar::cf() const
 975 {
 976         return Point(this->cfx(), this->cfy());
 977 }
 978
 979 Point
 980 BicycleCar::ccl() const
 981 {
 982         return Point(
 983                 this->x() + this->mtr() * cos(this->h() + M_PI / 2.0),
 984                 this->y() + this->mtr() * sin(this->h() + M_PI / 2.0)
 985         );
 986 }
 987
 988 Point
 989 BicycleCar::ccr() const
 990 {
 991         return Point(
 992                 this->x() + this->mtr() * cos(this->h() - M_PI / 2.0),
 993                 this->y() + this->mtr() * sin(this->h() - M_PI / 2.0)
 994         );
 995 }
 996
 997 void
 998 BicycleCar::next()
 999 {
1000         this->x(this->x() + this->sp() * cos(this->h()));
1001         this->y(this->y() + this->sp() * sin(this->h()));
1002         this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
1003 }
1004
1005 void
1006 BicycleCar::gen_gnuplot_to(std::ostream& out, GenPlotOpts opts)
1007 {
1008         if (opts.ALL) {
1009                 opts.CAR = true;
1010                 opts.MIRRORS = true;
1011         }
1012         if (opts.MIRRORS) {
1013                 opts.LEFT_MIRROR = true;
1014                 opts.RIGHT_MIRROR = true;
1015         }
1016         if (opts.CAR) {
1017                 opts.FRAME = true;
1018                 opts.CROSS = true;
1019                 opts.ARROW = true;
1020         }
1021         if (opts.FRAME) {
1022                 opts.LEFT = true;
1023                 opts.RIGHT = true;
1024                 opts.REAR = true;
1025                 opts.FRONT = true;
1026         }
1027         if (opts.LF_POINT) {
1028                 this->lf().gen_gnuplot_to(out);
1029         }
1030         if (opts.LR_POINT) {
1031                 this->lr().gen_gnuplot_to(out);
1032         }
1033         if (opts.RR_POINT) {
1034                 this->rr().gen_gnuplot_to(out);
1035         }
1036         if (opts.RF_POINT) {
1037                 this->rf().gen_gnuplot_to(out);
1038         }
1039         if (opts.LFM_POINT) {
1040                 this->lfm().gen_gnuplot_to(out);
1041         }
1042         if (opts.RFM_POINT) {
1043                 this->rfm().gen_gnuplot_to(out);
1044         }
1045         if (opts.CRA_POINT || opts.CAR_POINT) {
1046                 Point::gen_gnuplot_to(out);
1047         }
1048         if (opts.LRA_POINT) {
1049                 this->lra().gen_gnuplot_to(out);
1050         }
1051         if (opts.RRA_POINT) {
1052                 this->rra().gen_gnuplot_to(out);
1053         }
1054         if (opts.LEFT) {
1055                 this->lf().gen_gnuplot_to(out);
1056                 this->lr().gen_gnuplot_to(out);
1057                 out << std::endl;
1058         }
1059         if (opts.RIGHT) {
1060                 this->rf().gen_gnuplot_to(out);
1061                 this->rr().gen_gnuplot_to(out);
1062                 out << std::endl;
1063         }
1064         if (opts.REAR) {
1065                 this->lr().gen_gnuplot_to(out);
1066                 this->rr().gen_gnuplot_to(out);
1067                 out << std::endl;
1068         }
1069         if (opts.FRONT) {
1070                 this->lf().gen_gnuplot_to(out);
1071                 this->rf().gen_gnuplot_to(out);
1072                 out << std::endl;
1073         }
1074         if (opts.ARROW) {
1075                 this->cf().gen_gnuplot_to(out);
1076                 this->lfa().gen_gnuplot_to(out);
1077                 this->rfa().gen_gnuplot_to(out);
1078                 this->cf().gen_gnuplot_to(out);
1079                 out << std::endl;
1080         }
1081         if (opts.CROSS) {
1082                 double lx = this->x() + 0.2 * cos(this->h() + M_PI/2);
1083                 double rx = this->x() - 0.2 * cos(this->h() + M_PI/2);
1084                 double fx = this->x() + 0.2 * cos(this->h());
1085                 double bx = this->x() - 0.2 * cos(this->h()); // rear is back
1086                 double ly = this->y() + 0.2 * sin(this->h() + M_PI/2);
1087                 double ry = this->y() - 0.2 * sin(this->h() + M_PI/2);
1088                 double fy = this->y() + 0.2 * sin(this->h());
1089                 double by = this->y() - 0.2 * sin(this->h()); // rear is back
1090                 out << lx << " " << ly << std::endl;
1091                 out << rx << " " << ry << std::endl;
1092                 out << std::endl;
1093                 out << fx << " " << fy << std::endl;
1094                 out << bx << " " << by << std::endl;
1095                 out << std::endl;
1096         }
1097         if (opts.LEFT_MIRROR) {
1098                 this->lf().gen_gnuplot_to(out);
1099                 this->lfm().gen_gnuplot_to(out);
1100                 this->lr().gen_gnuplot_to(out);
1101                 out << std::endl;
1102
1103         }
1104         if (opts.RIGHT_MIRROR) {
1105                 this->rf().gen_gnuplot_to(out);
1106                 this->rfm().gen_gnuplot_to(out);
1107                 this->rr().gen_gnuplot_to(out);
1108                 out << std::endl;
1109         }
1110 }
1111
1112 } // namespace bcar