src/bcar.cc

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