src/bcar.cc

   1 /*
   2  * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 #include <cmath>
   8 #include "bcar.hh"
   9
  10 namespace bcar {
  11
  12 Point::Point()
  13 {
  14 }
  15
  16 Point::Point(double x, double y) : x_(x), y_(y)
  17 {
  18 }
  19
  20 double
  21 Point::x() const
  22 {
  23         return this->x_;
  24 }
  25
  26 void
  27 Point::x(double x)
  28 {
  29         this->x_ = x;
  30 }
  31
  32 double
  33 Point::y() const
  34 {
  35         return this->y_;
  36 }
  37
  38 void
  39 Point::y(double y)
  40 {
  41         this->y_ = y;
  42 }
  43
  44 double
  45 Point::min_angle_between(Point const& p1, Point const& p2) const
  46 {
  47         double d1x = p1.x() - this->x();
  48         double d1y = p1.y() - this->y();
  49         double d2x = p2.x() - p1.x();
  50         double d2y = p2.y() - p1.y();
  51
  52         double dot = d1x*d2x + d1y*d2y;
  53         double d1 = sqrt(d1x*d1x + d1y*d1y);
  54         double d2 = sqrt(d2x*d2x + d2y*d2y);
  55
  56         double delta = acos(dot / (d1 * d2));
  57         return std::min(delta, M_PI - delta);
  58 }
  59
  60 bool
  61 Point::inside_of(std::vector<Point> const& poly) const
  62 {
  63         unsigned int num = poly.size();
  64         unsigned int j = num - 1;
  65         bool c = false;
  66         for (unsigned int i = 0; i < num; i++) {
  67                 if (this->x() == poly[i].x() && this->y() == poly[i].y()) {
  68                         return true;
  69                 }
  70                 if ((poly[i].y() > this->y()) != (poly[j].y() > this->y())) {
  71                         auto slope1 = this->x() - poly[i].x();
  72                         slope1 *= poly[j].y() - poly[i].y();
  73                         auto slope2 = poly[j].x() - poly[i].x();
  74                         slope2 *= this->y() - poly[i].y();
  75                         auto slope = slope1 - slope2;
  76                         if (slope == 0.0) {
  77                                 return true;
  78                         }
  79                         if ((slope < 0.0) != (poly[j].y() < poly[i].y())) {
  80                                 c = !c;
  81                         }
  82                 }
  83                 j = i;
  84         }
  85         return c;
  86 }
  87
  88 bool
  89 Point::on_right_side_of(Line const& li) const
  90 {
  91         auto x1 = li.b().x();
  92         auto y1 = li.b().y();
  93         auto x2 = li.e().x();
  94         auto y2 = li.e().y();
  95         auto x3 = this->x_;
  96         auto y3 = this->y_;
  97         if (sgn((x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1)) < 0.0) {
  98                 return false;
  99         } else {
 100                 return true;
 101         }
 102 }
 103
 104 void
 105 Point::translate(Point const& p)
 106 {
 107         this->x_ += p.x();
 108         this->y_ += p.y();
 109 }
 110
 111 void
 112 Point::rotate(Point const& c, double const angl)
 113 {
 114         double px = this->x();
 115         double py = this->y();
 116         px -= c.x();
 117         py -= c.y();
 118         double nx = px * cos(angl) - py * sin(angl);
 119         double ny = px * sin(angl) + py * cos(angl);
 120         this->x(nx + c.x());
 121         this->y(ny + c.y());
 122 }
 123
 124 void
 125 Point::reflect(Line const& li)
 126 {
 127         this->rotate(li.b(), -li.h());
 128         this->y_ -= li.b().y();
 129         this->y_ *= -1.0;
 130         this->y_ += li.b().y();
 131         this->rotate(li.b(), li.h());
 132 }
 133
 134 double
 135 Point::edist(Point const& p) const
 136 {
 137         return sqrt(pow(p.x() - this->x_, 2.0) + pow(p.y() - this->y_, 2.0));
 138 }
 139
 140 bool
 141 Point::operator==(Point const& p)
 142 {
 143         return this->x() == p.x() && this->y() == p.y();
 144 }
 145
 146 std::ostream&
 147 operator<<(std::ostream& out, Point const& p)
 148 {
 149         out << "[" << p.x() << "," << p.y() << "]";
 150         return out;
 151 }
 152
 153 Line::Line(Point const& b, Point const& e): b_(b), e_(e)
 154 {
 155 }
 156
 157 Point
 158 Line::b() const&
 159 {
 160         return this->b_;
 161 }
 162
 163 Point
 164 Line::e() const&
 165 {
 166         return this->e_;
 167 }
 168
 169 Point
 170 Line::m() const
 171 {
 172         return Point((this->b_.x() + this->e_.x()) / 2.0,
 173                 (this->b_.y() + this->e_.y()) / 2.0);
 174 }
 175
 176 Point
 177 Line::i1() const&
 178 {
 179         return this->i1_;
 180 }
 181
 182 Point
 183 Line::i2() const&
 184 {
 185         return this->i2_;
 186 }
 187
 188 bool
 189 Line::intersects_with(Line const& li)
 190 {
 191         auto x1 = this->b_.x();
 192         auto y1 = this->b_.y();
 193         auto x2 = this->e_.x();
 194         auto y2 = this->e_.y();
 195         auto x3 = li.b().x();
 196         auto y3 = li.b().y();
 197         auto x4 = li.e().x();
 198         auto y4 = li.e().y();
 199         double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
 200         if (deno == 0.0) {
 201                 return false;
 202         }
 203         double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
 204         t /= deno;
 205         double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
 206         u *= -1.0;
 207         u /= deno;
 208         if (t < 0.0 || t > 1.0 || u < 0.0 || u > 1.0) {
 209                 return false;
 210         }
 211         this->i1_.x(x1 + t * (x2 - x1));
 212         this->i1_.y(y1 + t * (y2 - y1));
 213         return true;
 214 }
 215
 216 bool
 217 Line::intersects_with(Point const& c, double const r)
 218 {
 219         auto x1 = this->b_.x();
 220         auto y1 = this->b_.y();
 221         auto x2 = this->e_.x();
 222         auto y2 = this->e_.y();
 223         auto cx = c.x();
 224         auto cy = c.y();
 225         x2 -= cx;
 226         x1 -= cx;
 227         y2 -= cy;
 228         y1 -= cy;
 229         if (y1 == y2) {
 230                 y1 += 0.00001;
 231         }
 232         double dx = x2 - x1;
 233         double dy = y2 - y1;
 234         double dr = sqrt(dx*dx + dy*dy);
 235         double D = x1*y2 - x2*y1;
 236         if (r*r * dr*dr - D*D < 0.0) {
 237                 return false;
 238         }
 239         // intersection coordinates
 240         double ix1 = (D*dy + sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
 241         ix1 += cx;
 242         double ix2 = (D*dy - sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
 243         ix2 += cx;
 244         double iy1 = (-D*dx + std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
 245         iy1 += cy;
 246         double iy2 = (-D*dx - std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
 247         iy2 += cy;
 248         this->i1_.x(ix1);
 249         this->i1_.y(iy1);
 250         this->i2_.x(ix2);
 251         this->i2_.y(iy2);
 252         return true;
 253 }
 254
 255 double
 256 Line::len() const
 257 {
 258         return this->b_.edist(this->e_);
 259 }
 260
 261 double
 262 Line::h() const
 263 {
 264         return atan2(this->e_.y() - this->b_.y(), this->e_.x() - this->b_.x());
 265 }
 266
 267 std::ostream&
 268 operator<<(std::ostream& out, Line const& li)
 269 {
 270         out << "[" << li.b_ << "," << li.e_ << "]";
 271         return out;
 272 }
 273
 274 Pose::Pose(double x, double y, double h) : Point(x, y), h_(h)
 275 {
 276 }
 277
 278 double
 279 Pose::h() const
 280 {
 281         return this->h_;
 282 }
 283
 284 void
 285 Pose::h(double h)
 286 {
 287         while (h < -M_PI) {
 288                 h += 2 * M_PI;
 289         }
 290         while (h > +M_PI) {
 291                 h -= 2 * M_PI;
 292         }
 293         this->h_ = h;
 294 }
 295
 296 void
 297 Pose::set_pose(Pose const& p)
 298 {
 299         this->x(p.x());
 300         this->y(p.y());
 301         this->h(p.h());
 302 }
 303
 304 void
 305 Pose::rotate(Point const& c, double const angl)
 306 {
 307         Point::rotate(c, angl);
 308         this->h(this->h() + angl);
 309 }
 310
 311 void
 312 Pose::reflect(Line const& li)
 313 {
 314         Point::reflect(li);
 315         double dh = li.h() - this->h();
 316         this->h(this->h() + 2.0 * dh);
 317 }
 318
 319 bool
 320 Pose::operator==(Pose const& p)
 321 {
 322         return this->x() == p.x() && this->y() == p.y() && this->h() == p.h();
 323 }
 324
 325 std::ostream&
 326 operator<<(std::ostream& out, Pose const& p)
 327 {
 328         out << "[" << p.x() << "," << p.y() << "," << p.h() << "]";
 329         return out;
 330 }
 331
 332 void
 333 PoseRange::set_xyh()
 334 {
 335         double clen = 10.0;
 336         double bpbx = this->bp_.x() - clen * cos(this->bp_.h());
 337         double bpby = this->bp_.y() - clen * sin(this->bp_.h());
 338         double bpfx = this->bp_.x() + clen * cos(this->bp_.h());
 339         double bpfy = this->bp_.y() + clen * sin(this->bp_.h());
 340         Line li1(Point(bpbx, bpby), Point(bpfx, bpfy));
 341         double epbx = this->ep_.x() - clen * cos(this->ep_.h());
 342         double epby = this->ep_.y() - clen * sin(this->ep_.h());
 343         double epfx = this->ep_.x() + clen * cos(this->ep_.h());
 344         double epfy = this->ep_.y() + clen * sin(this->ep_.h());
 345         Line li2(Point(epbx, epby), Point(epfx, epfy));
 346         li1.intersects_with(li2);
 347         this->x(li1.i1().x());
 348         this->y(li1.i1().y());
 349         double bh = this->b();
 350         while (bh < 0.0) {
 351                 bh += 2.0 * M_PI;
 352         }
 353         this->bp_.h(bh);
 354         double eh = this->e();
 355         while (eh < 0.0) {
 356                 eh += 2.0 * M_PI;
 357         }
 358         this->ep_.h(eh);
 359         this->h((this->b() + this->e()) / 2.0);
 360 }
 361
 362 PoseRange::PoseRange(Pose bp, Pose ep) : bp_(bp), ep_(ep)
 363 {
 364         if (this->bp_ == this->ep_) {
 365                 this->set_pose(this->ep_);
 366         } else {
 367                 this->set_xyh();
 368         }
 369 }
 370
 371 PoseRange::PoseRange(double x, double y, double b, double e)
 372                 : PoseRange(Pose(x, y, b), Pose(x, y, e))
 373 {
 374 }
 375
 376 Pose
 377 PoseRange::bp() const
 378 {
 379         return this->bp_;
 380 }
 381
 382 Pose
 383 PoseRange::ep() const
 384 {
 385         return this->ep_;
 386 }
 387
 388 double
 389 PoseRange::b() const
 390 {
 391         return std::min(this->bp_.h(), this->ep_.h());
 392 }
 393
 394 double
 395 PoseRange::e() const
 396 {
 397         return std::max(this->bp_.h(), this->ep_.h());
 398 }
 399
 400 void
 401 PoseRange::translate(Point const& p)
 402 {
 403         this->bp_.translate(p);
 404         this->ep_.translate(p);
 405         this->set_xyh();
 406 }
 407
 408 void
 409 PoseRange::rotate(Point const& c, double const angl)
 410 {
 411         this->bp_.rotate(c, angl);
 412         this->ep_.rotate(c, angl);
 413         this->set_xyh();
 414 }
 415
 416 void
 417 PoseRange::reflect(Line const& li)
 418 {
 419         this->bp_.reflect(li);
 420         this->ep_.reflect(li);
 421         this->set_xyh();
 422 }
 423
 424 std::ostream&
 425 operator<<(std::ostream& out, PoseRange const& p)
 426 {
 427         out << "[" << p.x() << "," << p.y() << "," << p.b() << "," << p.e();
 428         out << "]";
 429         return out;
 430 }
 431
 432 double
 433 CarSize::ctc() const
 434 {
 435         return this->curb_to_curb_;
 436 }
 437
 438 void
 439 CarSize::ctc(double ctc)
 440 {
 441         this->curb_to_curb_ = ctc;
 442 }
 443
 444 double
 445 CarSize::wb() const
 446 {
 447         return this->wheelbase_;
 448 }
 449
 450 void
 451 CarSize::wb(double wb)
 452 {
 453         this->wheelbase_ = wb;
 454 }
 455
 456 double
 457 CarSize::w() const
 458 {
 459         return this->width_;
 460 }
 461
 462 void
 463 CarSize::w(double w)
 464 {
 465         this->width_ = w;
 466 }
 467
 468 double
 469 CarSize::len() const
 470 {
 471         return this->length_;
 472 }
 473
 474 void
 475 CarSize::len(double len)
 476 {
 477         this->length_ = len;
 478 }
 479
 480 double
 481 CarSize::df() const
 482 {
 483         return this->distance_to_front_;
 484 }
 485
 486 void
 487 CarSize::df(double df)
 488 {
 489         this->distance_to_front_ = df;
 490 }
 491
 492 double
 493 CarSize::dr() const
 494 {
 495         return this->len() - this->df();
 496 }
 497
 498 void
 499 CarSize::ft(double ft)
 500 {
 501         this->_front_track = ft;
 502 }
 503
 504 double
 505 CarSize::ft() const
 506 {
 507         return this->_front_track;
 508 }
 509
 510 double
 511 CarSize::mtr() const
 512 {
 513         auto ctc2 = pow(this->ctc() / 2.0, 2.0);
 514         auto wb2 = pow(this->wb(), 2.0);
 515         return sqrt(ctc2 - wb2) - this->ft() / 2.0;
 516 }
 517
 518 double
 519 CarSize::iradi() const
 520 {
 521         return this->mtr() - this->w() / 2;
 522 }
 523
 524 double
 525 CarSize::ofradi() const
 526 {
 527         auto mtrw2 = pow(this->mtr() + this->w() / 2.0, 2.0);
 528         auto df2 = pow(this->df(), 2.0);
 529         return sqrt(mtrw2 + df2);
 530 }
 531
 532 double
 533 CarSize::orradi() const
 534 {
 535         auto mtrw2 = pow(this->mtr() + this->w() / 2.0, 2.0);
 536         auto dr2 = pow(this->dr(), 2.0);
 537         return sqrt(mtrw2 + dr2);
 538 }
 539
 540 double
 541 CarSize::perfect_parking_slot_len() const
 542 {
 543         auto r = this->ctc() / 2.0;
 544         auto l = this->wb();
 545         auto k = this->df() - this->wb();
 546         auto w = this->w();
 547         auto r2l2 = r * r - l * l;
 548         auto s = r2l2 + pow(l + k, 2.0) - pow(sqrt(r2l2) - w, 2.0);
 549         return this->len() + sqrt(s) - l - k;
 550 }
 551
 552 double
 553 CarMove::sp() const
 554 {
 555         return this->speed_;
 556 }
 557
 558 void
 559 CarMove::sp(double sp)
 560 {
 561         this->speed_ = sp;
 562 }
 563
 564 double
 565 CarMove::st() const
 566 {
 567         return this->steer_;
 568 }
 569
 570 void
 571 CarMove::st(double st)
 572 {
 573         this->steer_ = st;
 574 }
 575
 576 bool
 577 BicycleCar::drivable(Pose const& p) const
 578 {
 579         return this->drivable(PoseRange(p, p));
 580 }
 581
 582 bool
 583 BicycleCar::drivable(PoseRange const& p) const
 584 {
 585         double a_1 = atan2(p.y() - this->y(), p.x() - this->x()) - this->h();
 586         while (a_1 < -M_PI)
 587                 a_1 += 2 * M_PI;
 588         while (a_1 > +M_PI)
 589                 a_1 -= 2 * M_PI;
 590         double h_d = p.h() - this->h();
 591         while (h_d < -M_PI)
 592                 h_d += 2 * M_PI;
 593         while (h_d > +M_PI)
 594                 h_d -= 2 * M_PI;
 595         double a_2 = 0;
 596         if (h_d == 0 && (a_1 == 0 || a_2 == M_PI || a_2 == -M_PI)) {
 597                 return true;
 598         } else if (0 < a_1 && a_1 <= M_PI/2) { // left front
 599                 BicycleCar z(*this); // zone border
 600                 z.h(p.e());
 601                 h_d = p.h() - this->h();
 602                 z.rotate(this->ccl(), h_d);
 603                 // assert z.h() == p.h()
 604                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 605                         return true;
 606                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 607                 while (a_2 < -M_PI)
 608                         a_2 += 2 * M_PI;
 609                 while (a_2 > +M_PI)
 610                         a_2 -= 2 * M_PI;
 611                 if (z.h() >= a_2 && a_2 >= this->h())
 612                         return true;
 613         } else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear
 614                 BicycleCar z(*this); // zone border
 615                 z.h(p.e());
 616                 h_d = p.h() - this->h();
 617                 z.rotate(this->ccl(), h_d);
 618                 // assert z.h() == p.h()
 619                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 620                         return true;
 621                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 622                 a_2 -= M_PI;
 623                 while (a_2 < -M_PI)
 624                         a_2 += 2 * M_PI;
 625                 while (a_2 > +M_PI)
 626                         a_2 -= 2 * M_PI;
 627                 if (this->h() >= a_2 && a_2 >= z.h())
 628                         return true;
 629         } else if (0 > a_1 && a_1 >= -M_PI/2) { // right front
 630                 BicycleCar z(*this); // zone border
 631                 z.h(p.b());
 632                 h_d = p.h() - this->h();
 633                 z.rotate(this->ccr(), h_d);
 634                 // assert z.h() == p.h()
 635                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 636                         return true;
 637                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 638                 while (a_2 < -M_PI)
 639                         a_2 += 2 * M_PI;
 640                 while (a_2 > +M_PI)
 641                         a_2 -= 2 * M_PI;
 642                 if (this->h() >= a_2 && a_2 >= z.h())
 643                         return true;
 644         } else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear
 645                 BicycleCar z(*this); // zone border
 646                 z.h(p.b());
 647                 h_d = p.h() - this->h();
 648                 z.rotate(this->ccr(), h_d);
 649                 // assert z.h() == p.h()
 650                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 651                         return true;
 652                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 653                 a_2 -= M_PI;
 654                 while (a_2 < -M_PI)
 655                         a_2 += 2 * M_PI;
 656                 while (a_2 > +M_PI)
 657                         a_2 -= 2 * M_PI;
 658                 if (z.h() >= a_2 && a_2 >= this->h())
 659                         return true;
 660         } else {
 661                 // Not happenning, as ``-pi <= a <= pi``.
 662         }
 663         return false;
 664 }
 665
 666 void
 667 BicycleCar::set_max_steer()
 668 {
 669         this->st(atan(this->wb() / this->mtr()));
 670 }
 671
 672 double
 673 BicycleCar::lfx() const
 674 {
 675         double lfx = this->x();
 676         lfx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0);
 677         lfx += this->df() * cos(this->h());
 678         return lfx;
 679 }
 680
 681 double
 682 BicycleCar::lfy() const
 683 {
 684         double lfy = this->y();
 685         lfy += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0);
 686         lfy += this->df() * sin(this->h());
 687         return lfy;
 688 }
 689
 690 double
 691 BicycleCar::lrx() const
 692 {
 693         double lrx = this->x();
 694         lrx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0);
 695         lrx += -this->dr() * cos(this->h());
 696         return lrx;
 697 }
 698
 699 double
 700 BicycleCar::lry() const
 701 {
 702         double lry = this->y();
 703         lry += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0);
 704         lry += -this->dr() * sin(this->h());
 705         return lry;
 706 }
 707
 708 double
 709 BicycleCar::rrx() const
 710 {
 711         double rrx = this->x();
 712         rrx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0);
 713         rrx += -this->dr() * cos(this->h());
 714         return rrx;
 715 }
 716
 717 double
 718 BicycleCar::rry() const
 719 {
 720         double rry = this->y();
 721         rry += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0);
 722         rry += -this->dr() * sin(this->h());
 723         return rry;
 724 }
 725
 726 double
 727 BicycleCar::rfx() const
 728 {
 729         double rfx = this->x();
 730         rfx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0);
 731         rfx += this->df() * cos(this->h());
 732         return rfx;
 733 }
 734
 735 double
 736 BicycleCar::rfy() const
 737 {
 738         double rfy = this->y();
 739         rfy += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0);
 740         rfy += this->df() * sin(this->h());
 741         return rfy;
 742 }
 743
 744 Point
 745 BicycleCar::lf() const
 746 {
 747         return Point(this->lfx(), this->lfy());
 748 }
 749
 750 Point
 751 BicycleCar::lr() const
 752 {
 753         return Point(this->lrx(), this->lry());
 754 }
 755
 756 Point
 757 BicycleCar::rr() const
 758 {
 759         return Point(this->rrx(), this->rry());
 760 }
 761
 762 Point
 763 BicycleCar::rf() const
 764 {
 765         return Point(this->rfx(), this->rfy());
 766 }
 767
 768 Line
 769 BicycleCar::left() const
 770 {
 771         return Line(this->lr(), this->lf());
 772 }
 773
 774 Line
 775 BicycleCar::rear() const
 776 {
 777         return Line(this->lr(), this->rr());
 778 }
 779
 780 Line
 781 BicycleCar::right() const
 782 {
 783         return Line(this->rr(), this->rf());
 784 }
 785
 786 Line
 787 BicycleCar::front() const
 788 {
 789         return Line(this->rf(), this->lf());
 790 }
 791
 792 double
 793 BicycleCar::ralx() const
 794 {
 795         double lrx = this->x();
 796         lrx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0);
 797         return lrx;
 798 }
 799 double
 800 BicycleCar::raly() const
 801 {
 802         double lry = this->y();
 803         lry += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0);
 804         return lry;
 805 }
 806
 807 double
 808 BicycleCar::rarx() const
 809 {
 810         double rrx = this->x();
 811         rrx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0);
 812         return rrx;
 813 }
 814
 815 double
 816 BicycleCar::rary() const
 817 {
 818         double rry = this->y();
 819         rry += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0);
 820         return rry;
 821 }
 822
 823 Point
 824 BicycleCar::ccl() const
 825 {
 826         return Point(
 827                 this->x() + this->mtr() * cos(this->h() + M_PI / 2.0),
 828                 this->y() + this->mtr() * sin(this->h() + M_PI / 2.0)
 829         );
 830 }
 831
 832 Point
 833 BicycleCar::ccr() const
 834 {
 835         return Point(
 836                 this->x() + this->mtr() * cos(this->h() - M_PI / 2.0),
 837                 this->y() + this->mtr() * sin(this->h() - M_PI / 2.0)
 838         );
 839 }
 840
 841 void
 842 BicycleCar::next()
 843 {
 844         this->x(this->x() + this->sp() * cos(this->h()));
 845         this->y(this->y() + this->sp() * sin(this->h()));
 846         this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
 847 }
 848
 849 } // namespace bcar