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(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(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::mtr() const
 546 {
 547         auto ctc2 = pow(this->ctc() / 2.0, 2.0);
 548         auto wb2 = pow(this->wb(), 2.0);
 549         return sqrt(ctc2 - wb2) - this->ft() / 2.0;
 550 }
 551
 552 double
 553 CarSize::iradi() const
 554 {
 555         return this->mtr() - this->w() / 2;
 556 }
 557
 558 double
 559 CarSize::ofradi() const
 560 {
 561         auto mtrw2 = pow(this->mtr() + this->w() / 2.0, 2.0);
 562         auto df2 = pow(this->df(), 2.0);
 563         return sqrt(mtrw2 + df2);
 564 }
 565
 566 double
 567 CarSize::orradi() const
 568 {
 569         auto mtrw2 = pow(this->mtr() + this->w() / 2.0, 2.0);
 570         auto dr2 = pow(this->dr(), 2.0);
 571         return sqrt(mtrw2 + dr2);
 572 }
 573
 574 double
 575 CarSize::imradi() 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::omradi() const
 584 {
 585         auto mtrw2 = pow(this->mtr() + this->wwm() / 2.0, 2.0);
 586         auto df2 = pow(this->wb(), 2.0);
 587         return sqrt(mtrw2 + df2);
 588 }
 589
 590 double
 591 CarSize::perfect_parking_slot_len() const
 592 {
 593         auto r = this->ctc() / 2.0;
 594         auto l = this->wb();
 595         auto k = this->df() - this->wb();
 596         auto w = this->w(); // FIXME use wwm()?
 597         auto r2l2 = r * r - l * l;
 598         auto s = r2l2 + pow(l + k, 2.0) - pow(sqrt(r2l2) - w, 2.0);
 599         return this->len() + sqrt(s) - l - k;
 600 }
 601
 602 double
 603 CarMove::sp() const
 604 {
 605         return this->_speed;
 606 }
 607
 608 void
 609 CarMove::sp(double sp)
 610 {
 611         this->_speed = sp;
 612 }
 613
 614 double
 615 CarMove::st() const
 616 {
 617         return this->_steer;
 618 }
 619
 620 void
 621 CarMove::st(double st)
 622 {
 623         this->_steer = st;
 624 }
 625
 626 bool
 627 BicycleCar::drivable(Pose const& p) const
 628 {
 629         return this->drivable(PoseRange(p, p));
 630 }
 631
 632 bool
 633 BicycleCar::drivable(PoseRange const& p) const
 634 {
 635         double a_1 = atan2(p.y() - this->y(), p.x() - this->x()) - this->h();
 636         while (a_1 < -M_PI)
 637                 a_1 += 2 * M_PI;
 638         while (a_1 > +M_PI)
 639                 a_1 -= 2 * M_PI;
 640         double h_d = p.h() - this->h();
 641         while (h_d < -M_PI)
 642                 h_d += 2 * M_PI;
 643         while (h_d > +M_PI)
 644                 h_d -= 2 * M_PI;
 645         double a_2 = 0;
 646         if (h_d == 0 && (a_1 == 0 || a_2 == M_PI || a_2 == -M_PI)) {
 647                 return true;
 648         } else if (0 < a_1 && a_1 <= M_PI/2) { // left front
 649                 BicycleCar z(*this); // zone border
 650                 z.h(p.e());
 651                 h_d = p.h() - this->h();
 652                 z.rotate(this->ccl(), h_d);
 653                 // assert z.h() == p.h()
 654                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 655                         return true;
 656                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 657                 while (a_2 < -M_PI)
 658                         a_2 += 2 * M_PI;
 659                 while (a_2 > +M_PI)
 660                         a_2 -= 2 * M_PI;
 661                 if (z.h() >= a_2 && a_2 >= this->h())
 662                         return true;
 663         } else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear
 664                 BicycleCar z(*this); // zone border
 665                 z.h(p.e());
 666                 h_d = p.h() - this->h();
 667                 z.rotate(this->ccl(), h_d);
 668                 // assert z.h() == p.h()
 669                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 670                         return true;
 671                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 672                 a_2 -= M_PI;
 673                 while (a_2 < -M_PI)
 674                         a_2 += 2 * M_PI;
 675                 while (a_2 > +M_PI)
 676                         a_2 -= 2 * M_PI;
 677                 if (this->h() >= a_2 && a_2 >= z.h())
 678                         return true;
 679         } else if (0 > a_1 && a_1 >= -M_PI/2) { // right front
 680                 BicycleCar z(*this); // zone border
 681                 z.h(p.b());
 682                 h_d = p.h() - this->h();
 683                 z.rotate(this->ccr(), h_d);
 684                 // assert z.h() == p.h()
 685                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 686                         return true;
 687                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 688                 while (a_2 < -M_PI)
 689                         a_2 += 2 * M_PI;
 690                 while (a_2 > +M_PI)
 691                         a_2 -= 2 * M_PI;
 692                 if (this->h() >= a_2 && a_2 >= z.h())
 693                         return true;
 694         } else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear
 695                 BicycleCar z(*this); // zone border
 696                 z.h(p.b());
 697                 h_d = p.h() - this->h();
 698                 z.rotate(this->ccr(), h_d);
 699                 // assert z.h() == p.h()
 700                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 701                         return true;
 702                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 703                 a_2 -= M_PI;
 704                 while (a_2 < -M_PI)
 705                         a_2 += 2 * M_PI;
 706                 while (a_2 > +M_PI)
 707                         a_2 -= 2 * M_PI;
 708                 if (z.h() >= a_2 && a_2 >= this->h())
 709                         return true;
 710         } else {
 711                 // Not happenning, as ``-pi <= a <= pi``.
 712         }
 713         return false;
 714 }
 715
 716 void
 717 BicycleCar::set_max_steer()
 718 {
 719         this->st(atan(this->wb() / this->mtr()));
 720 }
 721
 722 double
 723 BicycleCar::lfx() const
 724 {
 725         double lfx = this->x();
 726         lfx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0);
 727         lfx += this->df() * cos(this->h());
 728         return lfx;
 729 }
 730
 731 double
 732 BicycleCar::lfy() const
 733 {
 734         double lfy = this->y();
 735         lfy += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0);
 736         lfy += this->df() * sin(this->h());
 737         return lfy;
 738 }
 739
 740 double
 741 BicycleCar::lrx() const
 742 {
 743         double lrx = this->x();
 744         lrx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0);
 745         lrx += -this->dr() * cos(this->h());
 746         return lrx;
 747 }
 748
 749 double
 750 BicycleCar::lry() const
 751 {
 752         double lry = this->y();
 753         lry += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0);
 754         lry += -this->dr() * sin(this->h());
 755         return lry;
 756 }
 757
 758 double
 759 BicycleCar::rrx() const
 760 {
 761         double rrx = this->x();
 762         rrx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0);
 763         rrx += -this->dr() * cos(this->h());
 764         return rrx;
 765 }
 766
 767 double
 768 BicycleCar::rry() const
 769 {
 770         double rry = this->y();
 771         rry += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0);
 772         rry += -this->dr() * sin(this->h());
 773         return rry;
 774 }
 775
 776 double
 777 BicycleCar::rfx() const
 778 {
 779         double rfx = this->x();
 780         rfx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0);
 781         rfx += this->df() * cos(this->h());
 782         return rfx;
 783 }
 784
 785 double
 786 BicycleCar::rfy() const
 787 {
 788         double rfy = this->y();
 789         rfy += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0);
 790         rfy += this->df() * sin(this->h());
 791         return rfy;
 792 }
 793
 794 Point
 795 BicycleCar::lf() const
 796 {
 797         return Point(this->lfx(), this->lfy());
 798 }
 799
 800 Point
 801 BicycleCar::lr() const
 802 {
 803         return Point(this->lrx(), this->lry());
 804 }
 805
 806 Point
 807 BicycleCar::rr() const
 808 {
 809         return Point(this->rrx(), this->rry());
 810 }
 811
 812 Point
 813 BicycleCar::rf() const
 814 {
 815         return Point(this->rfx(), this->rfy());
 816 }
 817
 818 Line
 819 BicycleCar::left() const
 820 {
 821         return Line(this->lr(), this->lf());
 822 }
 823
 824 Line
 825 BicycleCar::rear() const
 826 {
 827         return Line(this->lr(), this->rr());
 828 }
 829
 830 Line
 831 BicycleCar::right() const
 832 {
 833         return Line(this->rr(), this->rf());
 834 }
 835
 836 Line
 837 BicycleCar::front() const
 838 {
 839         return Line(this->rf(), this->lf());
 840 }
 841
 842 double
 843 BicycleCar::lrax() const
 844 {
 845         double lrx = this->x();
 846         lrx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0);
 847         return lrx;
 848 }
 849 double
 850 BicycleCar::lray() const
 851 {
 852         double lry = this->y();
 853         lry += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0);
 854         return lry;
 855 }
 856
 857 double
 858 BicycleCar::rrax() const
 859 {
 860         double rrx = this->x();
 861         rrx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0);
 862         return rrx;
 863 }
 864
 865 double
 866 BicycleCar::rray() const
 867 {
 868         double rry = this->y();
 869         rry += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0);
 870         return rry;
 871 }
 872
 873 Point
 874 BicycleCar::lra() const
 875 {
 876         return Point(this->lrax(), this->lray());
 877 }
 878
 879 Point
 880 BicycleCar::rra() const
 881 {
 882         return Point(this->rrax(), this->rray());
 883 }
 884
 885 double
 886 BicycleCar::lfax() const
 887 {
 888         return this->lrax() + this->wb() * cos(this->h());
 889 }
 890
 891 double
 892 BicycleCar::lfay() const
 893 {
 894         return this->lray() + this->wb() * sin(this->h());
 895 }
 896
 897 double
 898 BicycleCar::rfax() const
 899 {
 900         return this->rrax() + this->wb() * cos(this->h());
 901 }
 902
 903 double
 904 BicycleCar::rfay() const
 905 {
 906         return this->rray() + this->wb() * sin(this->h());
 907 }
 908
 909 Point
 910 BicycleCar::lfa() const
 911 {
 912         return Point(this->lfax(), this->lfay());
 913 }
 914
 915 Point
 916 BicycleCar::rfa() const
 917 {
 918         return Point(this->rfax(), this->rfay());
 919 }
 920
 921 double
 922 BicycleCar::lfmx() const
 923 {
 924         double x = this->x();
 925         x += (this->wwm() / 2.0) * cos(this->h() + M_PI / 2.0);
 926         x += this->wb() * cos(this->h());
 927         return x;
 928 }
 929
 930 double
 931 BicycleCar::lfmy() const
 932 {
 933         double y = this->y();
 934         y += (this->wwm() / 2.0) * sin(this->h() + M_PI / 2.0);
 935         y += this->wb() * sin(this->h());
 936         return y;
 937 }
 938
 939 double
 940 BicycleCar::rfmx() const
 941 {
 942         double x = this->x();
 943         x += (this->wwm() / 2.0) * cos(this->h() - M_PI / 2.0);
 944         x += this->wb() * cos(this->h());
 945         return x;
 946 }
 947
 948 double
 949 BicycleCar::rfmy() const
 950 {
 951         double y = this->y();
 952         y += (this->wwm() / 2.0) * sin(this->h() - M_PI / 2.0);
 953         y += this->wb() * sin(this->h());
 954         return y;
 955 }
 956
 957 Point
 958 BicycleCar::lfm() const
 959 {
 960         return Point(this->lfmx(), this->lfmy());
 961 }
 962
 963 Point
 964 BicycleCar::rfm() const
 965 {
 966         return Point(this->rfmx(), this->rfmy());
 967 }
 968
 969 double
 970 BicycleCar::cfx() const
 971 {
 972         return this->x() + this->df() * cos(this->h());
 973 }
 974
 975 double
 976 BicycleCar::cfy() const
 977 {
 978         return this->y() + this->df() * sin(this->h());
 979 }
 980
 981 Point
 982 BicycleCar::cf() const
 983 {
 984         return Point(this->cfx(), this->cfy());
 985 }
 986
 987 Point
 988 BicycleCar::ccl() const
 989 {
 990         return Point(
 991                 this->x() + this->mtr() * cos(this->h() + M_PI / 2.0),
 992                 this->y() + this->mtr() * sin(this->h() + M_PI / 2.0)
 993         );
 994 }
 995
 996 Point
 997 BicycleCar::ccr() const
 998 {
 999         return Point(
1000                 this->x() + this->mtr() * cos(this->h() - M_PI / 2.0),
1001                 this->y() + this->mtr() * sin(this->h() - M_PI / 2.0)
1002         );
1003 }
1004
1005 void
1006 BicycleCar::next()
1007 {
1008         this->x(this->x() + this->sp() * cos(this->h()));
1009         this->y(this->y() + this->sp() * sin(this->h()));
1010         this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
1011 }
1012
1013 void
1014 BicycleCar::gen_gnuplot_to(std::ostream& out, GenPlotOpts opts)
1015 {
1016         if (opts.ALL) {
1017                 opts.CAR = true;
1018                 opts.MIRRORS = true;
1019         }
1020         if (opts.MIRRORS) {
1021                 opts.LEFT_MIRROR = true;
1022                 opts.RIGHT_MIRROR = true;
1023         }
1024         if (opts.CAR) {
1025                 opts.FRAME = true;
1026                 opts.CROSS = true;
1027                 opts.ARROW = true;
1028         }
1029         if (opts.FRAME) {
1030                 opts.LEFT = true;
1031                 opts.RIGHT = true;
1032                 opts.REAR = true;
1033                 opts.FRONT = true;
1034         }
1035         if (opts.LF_POINT) {
1036                 this->lf().gen_gnuplot_to(out);
1037         }
1038         if (opts.LR_POINT) {
1039                 this->lr().gen_gnuplot_to(out);
1040         }
1041         if (opts.RR_POINT) {
1042                 this->rr().gen_gnuplot_to(out);
1043         }
1044         if (opts.RF_POINT) {
1045                 this->rf().gen_gnuplot_to(out);
1046         }
1047         if (opts.LFM_POINT) {
1048                 this->lfm().gen_gnuplot_to(out);
1049         }
1050         if (opts.RFM_POINT) {
1051                 this->rfm().gen_gnuplot_to(out);
1052         }
1053         if (opts.CRA_POINT || opts.CAR_POINT) {
1054                 Point::gen_gnuplot_to(out);
1055         }
1056         if (opts.LRA_POINT) {
1057                 this->lra().gen_gnuplot_to(out);
1058         }
1059         if (opts.RRA_POINT) {
1060                 this->rra().gen_gnuplot_to(out);
1061         }
1062         if (opts.LEFT) {
1063                 this->lf().gen_gnuplot_to(out);
1064                 this->lr().gen_gnuplot_to(out);
1065                 out << std::endl;
1066         }
1067         if (opts.RIGHT) {
1068                 this->rf().gen_gnuplot_to(out);
1069                 this->rr().gen_gnuplot_to(out);
1070                 out << std::endl;
1071         }
1072         if (opts.REAR) {
1073                 this->lr().gen_gnuplot_to(out);
1074                 this->rr().gen_gnuplot_to(out);
1075                 out << std::endl;
1076         }
1077         if (opts.FRONT) {
1078                 this->lf().gen_gnuplot_to(out);
1079                 this->rf().gen_gnuplot_to(out);
1080                 out << std::endl;
1081         }
1082         if (opts.ARROW) {
1083                 this->cf().gen_gnuplot_to(out);
1084                 this->lfa().gen_gnuplot_to(out);
1085                 this->rfa().gen_gnuplot_to(out);
1086                 this->cf().gen_gnuplot_to(out);
1087                 out << std::endl;
1088         }
1089         if (opts.CROSS) {
1090                 double lx = this->x() + 0.2 * cos(this->h() + M_PI/2);
1091                 double rx = this->x() - 0.2 * cos(this->h() + M_PI/2);
1092                 double fx = this->x() + 0.2 * cos(this->h());
1093                 double bx = this->x() - 0.2 * cos(this->h()); // rear is back
1094                 double ly = this->y() + 0.2 * sin(this->h() + M_PI/2);
1095                 double ry = this->y() - 0.2 * sin(this->h() + M_PI/2);
1096                 double fy = this->y() + 0.2 * sin(this->h());
1097                 double by = this->y() - 0.2 * sin(this->h()); // rear is back
1098                 out << lx << " " << ly << std::endl;
1099                 out << rx << " " << ry << std::endl;
1100                 out << std::endl;
1101                 out << fx << " " << fy << std::endl;
1102                 out << bx << " " << by << std::endl;
1103                 out << std::endl;
1104         }
1105         if (opts.LEFT_MIRROR) {
1106                 this->lf().gen_gnuplot_to(out);
1107                 this->lfm().gen_gnuplot_to(out);
1108                 this->lr().gen_gnuplot_to(out);
1109                 out << std::endl;
1110
1111         }
1112         if (opts.RIGHT_MIRROR) {
1113                 this->rf().gen_gnuplot_to(out);
1114                 this->rfm().gen_gnuplot_to(out);
1115                 this->rr().gen_gnuplot_to(out);
1116                 out << std::endl;
1117         }
1118 }
1119
1120 } // namespace bcar