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::rotate(Point const& c, double const angl)
 100 {
 101         double px = this->x();
 102         double py = this->y();
 103         px -= c.x();
 104         py -= c.y();
 105         double nx = px * cos(angl) - py * sin(angl);
 106         double ny = px * sin(angl) + py * cos(angl);
 107         this->x(nx + c.x());
 108         this->y(ny + c.y());
 109 }
 110
 111 void
 112 Point::reflect(Line const& li)
 113 {
 114         this->rotate(li.b(), -li.h());
 115         this->y_ -= li.b().y();
 116         this->y_ *= -1.0;
 117         this->y_ += li.b().y();
 118         this->rotate(li.b(), li.h());
 119 }
 120
 121 double
 122 Point::edist(Point const& p) const
 123 {
 124         return sqrt(pow(p.x() - this->x_, 2.0) + pow(p.y() - this->y_, 2.0));
 125 }
 126
 127 std::ostream&
 128 operator<<(std::ostream& out, Point const& p)
 129 {
 130         out << "[" << p.x() << "," << p.y() << "]";
 131         return out;
 132 }
 133
 134 Line::Line(Point const& b, Point const& e): b_(b), e_(e)
 135 {
 136 }
 137
 138 Point
 139 Line::b() const&
 140 {
 141         return this->b_;
 142 }
 143
 144 Point
 145 Line::e() const&
 146 {
 147         return this->e_;
 148 }
 149
 150 Point
 151 Line::i1() const&
 152 {
 153         return this->i1_;
 154 }
 155
 156 Point
 157 Line::i2() const&
 158 {
 159         return this->i2_;
 160 }
 161
 162 bool
 163 Line::intersects_with(Line const& li)
 164 {
 165         auto x1 = this->b_.x();
 166         auto y1 = this->b_.y();
 167         auto x2 = this->e_.x();
 168         auto y2 = this->e_.y();
 169         auto x3 = li.b().x();
 170         auto y3 = li.b().y();
 171         auto x4 = li.e().x();
 172         auto y4 = li.e().y();
 173         double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
 174         if (deno == 0.0) {
 175                 return false;
 176         }
 177         double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
 178         t /= deno;
 179         double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
 180         u *= -1.0;
 181         u /= deno;
 182         if (t < 0.0 || t > 1.0 || u < 0.0 || u > 1.0) {
 183                 return false;
 184         }
 185         this->i1_.x(x1 + t * (x2 - x1));
 186         this->i1_.y(y1 + t * (y2 - y1));
 187         return true;
 188 }
 189
 190 bool
 191 Line::intersects_with(Point const& c, double const r)
 192 {
 193         auto x1 = this->b_.x();
 194         auto y1 = this->b_.y();
 195         auto x2 = this->e_.x();
 196         auto y2 = this->e_.y();
 197         auto cx = c.x();
 198         auto cy = c.y();
 199         x2 -= cx;
 200         x1 -= cx;
 201         y2 -= cy;
 202         y1 -= cy;
 203         if (y1 == y2) {
 204                 y1 += 0.00001;
 205         }
 206         double dx = x2 - x1;
 207         double dy = y2 - y1;
 208         double dr = sqrt(dx*dx + dy*dy);
 209         double D = x1*y2 - x2*y1;
 210         if (r*r * dr*dr - D*D < 0.0) {
 211                 return false;
 212         }
 213         // intersection coordinates
 214         double ix1 = (D*dy + sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
 215         ix1 += cx;
 216         double ix2 = (D*dy - sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
 217         ix2 += cx;
 218         double iy1 = (-D*dx + std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
 219         iy1 += cy;
 220         double iy2 = (-D*dx - std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
 221         iy2 += cy;
 222         this->i1_.x(ix1);
 223         this->i1_.y(iy1);
 224         this->i2_.x(ix2);
 225         this->i2_.y(iy2);
 226         return true;
 227 }
 228
 229 double
 230 Line::len() const
 231 {
 232         return this->b_.edist(this->e_);
 233 }
 234
 235 double
 236 Line::h() const
 237 {
 238         return atan2(this->e_.y() - this->b_.y(), this->e_.x() - this->b_.x());
 239 }
 240
 241 std::ostream&
 242 operator<<(std::ostream& out, Line const& li)
 243 {
 244         out << "[" << li.b_ << "," << li.e_ << "]";
 245         return out;
 246 }
 247
 248 Pose::Pose(double x, double y, double h) : Point(x, y), h_(h)
 249 {
 250 }
 251
 252 double
 253 Pose::h() const
 254 {
 255         return this->h_;
 256 }
 257
 258 void
 259 Pose::h(double h)
 260 {
 261         while (h < -M_PI) {
 262                 h += 2 * M_PI;
 263         }
 264         while (h > +M_PI) {
 265                 h -= 2 * M_PI;
 266         }
 267         this->h_ = h;
 268 }
 269
 270 void
 271 Pose::set_pose(Pose const& p)
 272 {
 273         this->x(p.x());
 274         this->y(p.y());
 275         this->h(p.h());
 276 }
 277
 278 void
 279 Pose::rotate(Point const& c, double const angl)
 280 {
 281         Point::rotate(c, angl);
 282         this->h(this->h() + angl);
 283 }
 284
 285 void
 286 Pose::reflect(Line const& li)
 287 {
 288         Point::reflect(li);
 289         double dh = li.h() - this->h();
 290         this->h(this->h() + 2.0 * dh);
 291 }
 292
 293 std::ostream&
 294 operator<<(std::ostream& out, Pose const& p)
 295 {
 296         out << "[" << p.x() << "," << p.y() << "," << p.h() << "]";
 297         return out;
 298 }
 299
 300 double
 301 PoseRange::b() const
 302 {
 303         return this->h();
 304 }
 305
 306 void
 307 PoseRange::b(double b)
 308 {
 309         this->h(b);
 310 }
 311
 312 double
 313 PoseRange::e() const
 314 {
 315         return this->e_;
 316 }
 317
 318 void
 319 PoseRange::e(double e)
 320 {
 321         while (e < -M_PI) {
 322                 e += 2 * M_PI;
 323         }
 324         while (e > +M_PI) {
 325                 e -= 2 * M_PI;
 326         }
 327         this->e_ = e;
 328 }
 329
 330 void
 331 PoseRange::rotate(Point const& c, double const angl)
 332 {
 333         Pose::rotate(c, angl);
 334         this->e(this->e() + angl);
 335 }
 336
 337 void
 338 PoseRange::reflect(Line const& li)
 339 {
 340         Pose::reflect(li);
 341         double dh = li.h() - this->e();
 342         this->e(this->e() + 2.0 * dh);
 343 }
 344
 345 std::ostream&
 346 operator<<(std::ostream& out, PoseRange const& p)
 347 {
 348         out << "[" << p.x() << "," << p.y() << "," << p.b() << "," << p.e();
 349         out << "]";
 350         return out;
 351 }
 352
 353 double
 354 CarSize::ctc() const
 355 {
 356         return this->curb_to_curb_;
 357 }
 358
 359 void
 360 CarSize::ctc(double ctc)
 361 {
 362         this->curb_to_curb_ = ctc;
 363 }
 364
 365 double
 366 CarSize::wb() const
 367 {
 368         return this->wheelbase_;
 369 }
 370
 371 void
 372 CarSize::wb(double wb)
 373 {
 374         this->wheelbase_ = wb;
 375 }
 376
 377 double
 378 CarSize::w() const
 379 {
 380         return this->width_;
 381 }
 382
 383 void
 384 CarSize::w(double w)
 385 {
 386         this->width_ = w;
 387 }
 388
 389 double
 390 CarSize::len() const
 391 {
 392         return this->length_;
 393 }
 394
 395 void
 396 CarSize::len(double len)
 397 {
 398         this->length_ = len;
 399 }
 400
 401 double
 402 CarSize::df() const
 403 {
 404         return this->distance_to_front_;
 405 }
 406
 407 void
 408 CarSize::df(double df)
 409 {
 410         this->distance_to_front_ = df;
 411 }
 412
 413 double
 414 CarSize::dr() const
 415 {
 416         return this->len() - this->df();
 417 }
 418
 419 double
 420 CarSize::mtr() const
 421 {
 422         auto ctc2 = pow(this->ctc() / 2.0, 2.0);
 423         auto wb2 = pow(this->wb(), 2.0);
 424         return sqrt(ctc2 - wb2) - this->w() / 2.0;
 425 }
 426
 427 double
 428 CarSize::iradi() const
 429 {
 430         return this->mtr() - this->w() / 2;
 431 }
 432
 433 double
 434 CarSize::ofradi() const
 435 {
 436         auto mtrw2 = pow(this->mtr() + this->w() / 2.0, 2.0);
 437         auto df2 = pow(this->df(), 2.0);
 438         return sqrt(mtrw2 + df2);
 439 }
 440
 441 double
 442 CarSize::orradi() const
 443 {
 444         auto mtrw2 = pow(this->mtr() + this->w() / 2.0, 2.0);
 445         auto dr2 = pow(this->dr(), 2.0);
 446         return sqrt(mtrw2 + dr2);
 447 }
 448
 449 double
 450 CarSize::perfect_parking_slot_len() const
 451 {
 452         auto r = this->ctc() / 2.0;
 453         auto l = this->wb();
 454         auto k = this->df() - this->wb();
 455         auto w = this->w();
 456         auto r2l2 = r * r - l * l;
 457         auto s = r2l2 + pow(l + k, 2.0) - pow(sqrt(r2l2) - w, 2.0);
 458         return this->len() + sqrt(s) - l - k;
 459 }
 460
 461 double
 462 CarMove::sp() const
 463 {
 464         return this->speed_;
 465 }
 466
 467 void
 468 CarMove::sp(double sp)
 469 {
 470         this->speed_ = sp;
 471 }
 472
 473 double
 474 CarMove::st() const
 475 {
 476         return this->steer_;
 477 }
 478
 479 void
 480 CarMove::st(double st)
 481 {
 482         this->steer_ = st;
 483 }
 484
 485 bool
 486 BicycleCar::drivable(Pose const& p) const
 487 {
 488         PoseRange pr;
 489         pr.x(p.x());
 490         pr.y(p.y());
 491         pr.b(p.h());
 492         pr.e(p.h());
 493         return this->drivable(pr);
 494 }
 495
 496 bool
 497 BicycleCar::drivable(PoseRange const& p) const
 498 {
 499         double h = (p.b() + p.e()) / 2.0;
 500         double a_1 = atan2(p.y() - this->y(), p.x() - this->x()) - this->h();
 501         while (a_1 < -M_PI)
 502                 a_1 += 2 * M_PI;
 503         while (a_1 > +M_PI)
 504                 a_1 -= 2 * M_PI;
 505         double h_d = h - this->h();
 506         while (h_d < -M_PI)
 507                 h_d += 2 * M_PI;
 508         while (h_d > +M_PI)
 509                 h_d -= 2 * M_PI;
 510         double a_2 = 0;
 511         if (h_d == 0 && (a_1 == 0 || a_2 == M_PI || a_2 == -M_PI)) {
 512                 return true;
 513         } else if (0 < a_1 && a_1 <= M_PI/2) { // left front
 514                 BicycleCar z(*this); // zone border
 515                 z.h(p.e());
 516                 h_d = h - this->h();
 517                 z.rotate(this->ccl(), h_d);
 518                 // assert z.h() == h
 519                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 520                         return true;
 521                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 522                 while (a_2 < -M_PI)
 523                         a_2 += 2 * M_PI;
 524                 while (a_2 > +M_PI)
 525                         a_2 -= 2 * M_PI;
 526                 if (z.h() >= a_2 && a_2 >= this->h())
 527                         return true;
 528         } else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear
 529                 BicycleCar z(*this); // zone border
 530                 z.h(p.e());
 531                 h_d = h - this->h();
 532                 z.rotate(this->ccl(), h_d);
 533                 // assert z.h() == h
 534                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 535                         return true;
 536                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 537                 a_2 -= M_PI;
 538                 while (a_2 < -M_PI)
 539                         a_2 += 2 * M_PI;
 540                 while (a_2 > +M_PI)
 541                         a_2 -= 2 * M_PI;
 542                 if (this->h() >= a_2 && a_2 >= z.h())
 543                         return true;
 544         } else if (0 > a_1 && a_1 >= -M_PI/2) { // right front
 545                 BicycleCar z(*this); // zone border
 546                 z.h(p.b());
 547                 h_d = h - this->h();
 548                 z.rotate(this->ccr(), h_d);
 549                 // assert z.h() == h
 550                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 551                         return true;
 552                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 553                 while (a_2 < -M_PI)
 554                         a_2 += 2 * M_PI;
 555                 while (a_2 > +M_PI)
 556                         a_2 -= 2 * M_PI;
 557                 if (this->h() >= a_2 && a_2 >= z.h())
 558                         return true;
 559         } else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear
 560                 BicycleCar z(*this); // zone border
 561                 z.h(p.b());
 562                 h_d = h - this->h();
 563                 z.rotate(this->ccr(), h_d);
 564                 // assert z.h() == h
 565                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 566                         return true;
 567                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 568                 a_2 -= M_PI;
 569                 while (a_2 < -M_PI)
 570                         a_2 += 2 * M_PI;
 571                 while (a_2 > +M_PI)
 572                         a_2 -= 2 * M_PI;
 573                 if (z.h() >= a_2 && a_2 >= this->h())
 574                         return true;
 575         } else {
 576                 // Not happenning, as ``-pi <= a <= pi``.
 577         }
 578         return false;
 579 }
 580
 581 void
 582 BicycleCar::set_max_steer()
 583 {
 584         this->st(atan(this->wb() / this->mtr()));
 585 }
 586
 587 double
 588 BicycleCar::lfx() const
 589 {
 590         double lfx = this->x();
 591         lfx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0);
 592         lfx += this->df() * cos(this->h());
 593         return lfx;
 594 }
 595
 596 double
 597 BicycleCar::lfy() const
 598 {
 599         double lfy = this->y();
 600         lfy += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0);
 601         lfy += this->df() * sin(this->h());
 602         return lfy;
 603 }
 604
 605 double
 606 BicycleCar::lrx() const
 607 {
 608         double lrx = this->x();
 609         lrx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0);
 610         lrx += -this->dr() * cos(this->h());
 611         return lrx;
 612 }
 613
 614 double
 615 BicycleCar::lry() const
 616 {
 617         double lry = this->y();
 618         lry += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0);
 619         lry += -this->dr() * sin(this->h());
 620         return lry;
 621 }
 622
 623 double
 624 BicycleCar::rrx() const
 625 {
 626         double rrx = this->x();
 627         rrx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0);
 628         rrx += -this->dr() * cos(this->h());
 629         return rrx;
 630 }
 631
 632 double
 633 BicycleCar::rry() const
 634 {
 635         double rry = this->y();
 636         rry += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0);
 637         rry += -this->dr() * sin(this->h());
 638         return rry;
 639 }
 640
 641 double
 642 BicycleCar::rfx() const
 643 {
 644         double rfx = this->x();
 645         rfx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0);
 646         rfx += this->df() * cos(this->h());
 647         return rfx;
 648 }
 649
 650 double
 651 BicycleCar::rfy() const
 652 {
 653         double rfy = this->y();
 654         rfy += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0);
 655         rfy += this->df() * sin(this->h());
 656         return rfy;
 657 }
 658
 659 Point
 660 BicycleCar::lf() const
 661 {
 662         return Point(this->lfx(), this->lfy());
 663 }
 664
 665 Point
 666 BicycleCar::lr() const
 667 {
 668         return Point(this->lrx(), this->lry());
 669 }
 670
 671 Point
 672 BicycleCar::rr() const
 673 {
 674         return Point(this->rrx(), this->rry());
 675 }
 676
 677 Point
 678 BicycleCar::rf() const
 679 {
 680         return Point(this->rfx(), this->rfy());
 681 }
 682
 683 Line
 684 BicycleCar::left() const
 685 {
 686         return Line(this->lr(), this->lf());
 687 }
 688
 689 Line
 690 BicycleCar::rear() const
 691 {
 692         return Line(this->lr(), this->rr());
 693 }
 694
 695 Line
 696 BicycleCar::right() const
 697 {
 698         return Line(this->rr(), this->rf());
 699 }
 700
 701 Line
 702 BicycleCar::front() const
 703 {
 704         return Line(this->rf(), this->lf());
 705 }
 706
 707 double
 708 BicycleCar::ralx() const
 709 {
 710         double lrx = this->x();
 711         lrx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0);
 712         return lrx;
 713 }
 714 double
 715 BicycleCar::raly() const
 716 {
 717         double lry = this->y();
 718         lry += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0);
 719         return lry;
 720 }
 721
 722 double
 723 BicycleCar::rarx() const
 724 {
 725         double rrx = this->x();
 726         rrx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0);
 727         return rrx;
 728 }
 729
 730 double
 731 BicycleCar::rary() const
 732 {
 733         double rry = this->y();
 734         rry += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0);
 735         return rry;
 736 }
 737
 738 Point
 739 BicycleCar::ccl() const
 740 {
 741         return Point(
 742                 this->x() + this->mtr() * cos(this->h() + M_PI / 2.0),
 743                 this->y() + this->mtr() * sin(this->h() + M_PI / 2.0)
 744         );
 745 }
 746
 747 Point
 748 BicycleCar::ccr() const
 749 {
 750         return Point(
 751                 this->x() + this->mtr() * cos(this->h() - M_PI / 2.0),
 752                 this->y() + this->mtr() * sin(this->h() - M_PI / 2.0)
 753         );
 754 }
 755
 756 void
 757 BicycleCar::next()
 758 {
 759         this->x(this->x() + this->sp() * cos(this->h()));
 760         this->y(this->y() + this->sp() * sin(this->h()));
 761         this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
 762 }
 763
 764 } // namespace bcar