src/bcar.cc

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