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