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