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