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