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