src/bcar.cc

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