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::rotate(Point const& c, double const angl)
 238 {
 239         double px = this->x();
 240         double py = this->y();
 241         px -= c.x();
 242         py -= c.y();
 243         double nx = px * cos(angl) - py * sin(angl);
 244         double ny = px * sin(angl) + py * cos(angl);
 245         this->h(this->h() + angl);
 246         this->x(nx + c.x());
 247         this->y(ny + c.y());
 248 }
 249
 250 std::ostream&
 251 operator<<(std::ostream& out, Pose const& p)
 252 {
 253         out << "[" << p.x() << "," << p.y() << "," << p.h() << "]";
 254         return out;
 255 }
 256
 257 double
 258 PoseRange::b() const
 259 {
 260         return this->h();
 261 }
 262
 263 void
 264 PoseRange::b(double b)
 265 {
 266         this->h(b);
 267 }
 268
 269 double
 270 PoseRange::e() const
 271 {
 272         return this->e_;
 273 }
 274
 275 void
 276 PoseRange::e(double e)
 277 {
 278         while (e < -M_PI) {
 279                 e += 2 * M_PI;
 280         }
 281         while (e > +M_PI) {
 282                 e -= 2 * M_PI;
 283         }
 284         this->e_ = e;
 285 }
 286
 287 void
 288 PoseRange::rotate(Point const& c, double const angl)
 289 {
 290         Pose::rotate(c, angl);
 291         this->e(this->e() + angl);
 292 }
 293
 294 std::ostream&
 295 operator<<(std::ostream& out, PoseRange const& p)
 296 {
 297         out << "[" << p.x() << "," << p.y() << "," << p.b() << "," << p.e();
 298         out << "]";
 299         return out;
 300 }
 301
 302 double
 303 CarSize::ctc() const
 304 {
 305         return this->curb_to_curb;
 306 }
 307
 308 void
 309 CarSize::ctc(double ctc)
 310 {
 311         this->curb_to_curb = ctc;
 312 }
 313
 314 double
 315 CarSize::wb() const
 316 {
 317         return this->wheelbase;
 318 }
 319
 320 void
 321 CarSize::wb(double wb)
 322 {
 323         this->wheelbase = wb;
 324 }
 325
 326 double
 327 CarSize::w() const
 328 {
 329         return this->width;
 330 }
 331
 332 void
 333 CarSize::w(double w)
 334 {
 335         this->width = w;
 336 }
 337
 338 double
 339 CarSize::len() const
 340 {
 341         return this->length;
 342 }
 343
 344 void
 345 CarSize::len(double len)
 346 {
 347         this->length = len;
 348 }
 349
 350 double
 351 CarSize::df() const
 352 {
 353         return this->distance_to_front;
 354 }
 355
 356 void
 357 CarSize::df(double df)
 358 {
 359         this->distance_to_front = df;
 360 }
 361
 362 double
 363 CarSize::dr() const
 364 {
 365         return this->len() - this->df();
 366 }
 367
 368 double
 369 CarSize::mtr() const
 370 {
 371         auto ctc2 = pow(this->ctc() / 2.0, 2.0);
 372         auto wb2 = pow(this->wb(), 2.0);
 373         return sqrt(ctc2 - wb2) - this->w() / 2.0;
 374 }
 375
 376 double
 377 CarMove::sp() const
 378 {
 379         return this->speed;
 380 }
 381
 382 void
 383 CarMove::sp(double sp)
 384 {
 385         this->speed = sp;
 386 }
 387
 388 double
 389 CarMove::st() const
 390 {
 391         return this->steer;
 392 }
 393
 394 void
 395 CarMove::st(double st)
 396 {
 397         this->steer = st;
 398 }
 399
 400 bool
 401 BicycleCar::drivable(Pose const& p) const
 402 {
 403         PoseRange pr;
 404         pr.x(p.x());
 405         pr.y(p.y());
 406         pr.b(p.h());
 407         pr.e(p.h());
 408         return this->drivable(pr);
 409 }
 410
 411 bool
 412 BicycleCar::drivable(PoseRange const& p) const
 413 {
 414         double h = (p.b() + p.e()) / 2.0;
 415         double a_1 = atan2(p.y() - this->y(), p.x() - this->x()) - this->h();
 416         while (a_1 < -M_PI)
 417                 a_1 += 2 * M_PI;
 418         while (a_1 > +M_PI)
 419                 a_1 -= 2 * M_PI;
 420         double h_d = h - this->h();
 421         while (h_d < -M_PI)
 422                 h_d += 2 * M_PI;
 423         while (h_d > +M_PI)
 424                 h_d -= 2 * M_PI;
 425         double a_2 = 0;
 426         if (h_d == 0 && (a_1 == 0 || a_2 == M_PI || a_2 == -M_PI)) {
 427                 return true;
 428         } else if (0 < a_1 && a_1 <= M_PI/2) { // left front
 429                 BicycleCar z(*this); // zone border
 430                 z.h(p.e());
 431                 h_d = h - this->h();
 432                 z.rotate(this->ccl(), h_d);
 433                 // assert z.h() == h
 434                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 435                         return true;
 436                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 437                 while (a_2 < -M_PI)
 438                         a_2 += 2 * M_PI;
 439                 while (a_2 > +M_PI)
 440                         a_2 -= 2 * M_PI;
 441                 if (z.h() >= a_2 && a_2 >= this->h())
 442                         return true;
 443         } else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear
 444                 BicycleCar z(*this); // zone border
 445                 z.h(p.e());
 446                 h_d = h - this->h();
 447                 z.rotate(this->ccl(), h_d);
 448                 // assert z.h() == h
 449                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 450                         return true;
 451                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 452                 a_2 -= M_PI;
 453                 while (a_2 < -M_PI)
 454                         a_2 += 2 * M_PI;
 455                 while (a_2 > +M_PI)
 456                         a_2 -= 2 * M_PI;
 457                 if (this->h() >= a_2 && a_2 >= z.h())
 458                         return true;
 459         } else if (0 > a_1 && a_1 >= -M_PI/2) { // right front
 460                 BicycleCar z(*this); // zone border
 461                 z.h(p.b());
 462                 h_d = h - this->h();
 463                 z.rotate(this->ccr(), h_d);
 464                 // assert z.h() == h
 465                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 466                         return true;
 467                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 468                 while (a_2 < -M_PI)
 469                         a_2 += 2 * M_PI;
 470                 while (a_2 > +M_PI)
 471                         a_2 -= 2 * M_PI;
 472                 if (this->h() >= a_2 && a_2 >= z.h())
 473                         return true;
 474         } else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear
 475                 BicycleCar z(*this); // zone border
 476                 z.h(p.b());
 477                 h_d = h - this->h();
 478                 z.rotate(this->ccr(), h_d);
 479                 // assert z.h() == h
 480                 if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 481                         return true;
 482                 a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 483                 a_2 -= M_PI;
 484                 while (a_2 < -M_PI)
 485                         a_2 += 2 * M_PI;
 486                 while (a_2 > +M_PI)
 487                         a_2 -= 2 * M_PI;
 488                 if (z.h() >= a_2 && a_2 >= this->h())
 489                         return true;
 490         } else {
 491                 // Not happenning, as ``-pi <= a <= pi``.
 492         }
 493         return false;
 494 }
 495
 496 double
 497 BicycleCar::iradi() const
 498 {
 499         return this->mtr() - this->w() / 2;
 500 }
 501
 502 double
 503 BicycleCar::ofradi() const
 504 {
 505         auto mtrw2 = pow(this->mtr() + this->w() / 2.0, 2.0);
 506         auto df2 = pow(this->df(), 2.0);
 507         return sqrt(mtrw2 + df2);
 508 }
 509
 510 double
 511 BicycleCar::orradi() const
 512 {
 513         auto mtrw2 = pow(this->mtr() + this->w() / 2.0, 2.0);
 514         auto dr2 = pow(this->dr(), 2.0);
 515         return sqrt(mtrw2 + dr2);
 516 }
 517
 518 double
 519 BicycleCar::perfect_parking_slot_len() const
 520 {
 521         auto r = this->ctc() / 2.0;
 522         auto l = this->wb();
 523         auto k = this->df() - this->wb();
 524         auto w = this->w();
 525         auto r2l2 = r * r - l * l;
 526         auto s = r2l2 + pow(l + k, 2.0) - pow(sqrt(r2l2) - w, 2.0);
 527         return this->len() + sqrt(s) - l - k;
 528 }
 529
 530 void
 531 BicycleCar::set_max_steer()
 532 {
 533         this->st(atan(this->wb() / this->mtr()));
 534 }
 535
 536 double
 537 BicycleCar::lfx() const
 538 {
 539         double lfx = this->x();
 540         lfx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0);
 541         lfx += this->df() * cos(this->h());
 542         return lfx;
 543 }
 544
 545 double
 546 BicycleCar::lfy() const
 547 {
 548         double lfy = this->y();
 549         lfy += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0);
 550         lfy += this->df() * sin(this->h());
 551         return lfy;
 552 }
 553
 554 double
 555 BicycleCar::lrx() const
 556 {
 557         double lrx = this->x();
 558         lrx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0);
 559         lrx += -this->dr() * cos(this->h());
 560         return lrx;
 561 }
 562
 563 double
 564 BicycleCar::lry() const
 565 {
 566         double lry = this->y();
 567         lry += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0);
 568         lry += -this->dr() * sin(this->h());
 569         return lry;
 570 }
 571
 572 double
 573 BicycleCar::rrx() const
 574 {
 575         double rrx = this->x();
 576         rrx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0);
 577         rrx += -this->dr() * cos(this->h());
 578         return rrx;
 579 }
 580
 581 double
 582 BicycleCar::rry() const
 583 {
 584         double rry = this->y();
 585         rry += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0);
 586         rry += -this->dr() * sin(this->h());
 587         return rry;
 588 }
 589
 590 double
 591 BicycleCar::rfx() const
 592 {
 593         double rfx = this->x();
 594         rfx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0);
 595         rfx += this->df() * cos(this->h());
 596         return rfx;
 597 }
 598
 599 double
 600 BicycleCar::rfy() const
 601 {
 602         double rfy = this->y();
 603         rfy += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0);
 604         rfy += this->df() * sin(this->h());
 605         return rfy;
 606 }
 607
 608 double
 609 BicycleCar::ralx() const
 610 {
 611         double lrx = this->x();
 612         lrx += (this->w() / 2.0) * cos(this->h() + M_PI / 2.0);
 613         return lrx;
 614 }
 615 double
 616 BicycleCar::raly() const
 617 {
 618         double lry = this->y();
 619         lry += (this->w() / 2.0) * sin(this->h() + M_PI / 2.0);
 620         return lry;
 621 }
 622
 623 double
 624 BicycleCar::rarx() const
 625 {
 626         double rrx = this->x();
 627         rrx += (this->w() / 2.0) * cos(this->h() - M_PI / 2.0);
 628         return rrx;
 629 }
 630
 631 double
 632 BicycleCar::rary() const
 633 {
 634         double rry = this->y();
 635         rry += (this->w() / 2.0) * sin(this->h() - M_PI / 2.0);
 636         return rry;
 637 }
 638
 639 Point
 640 BicycleCar::ccl() const
 641 {
 642         return Point(
 643                 this->x() + this->mtr() * cos(this->h() + M_PI / 2.0),
 644                 this->y() + this->mtr() * sin(this->h() + M_PI / 2.0)
 645         );
 646 }
 647
 648 Point
 649 BicycleCar::ccr() const
 650 {
 651         return Point(
 652                 this->x() + this->mtr() * cos(this->h() - M_PI / 2.0),
 653                 this->y() + this->mtr() * sin(this->h() - M_PI / 2.0)
 654         );
 655 }
 656
 657 void
 658 BicycleCar::next()
 659 {
 660         this->x(this->x() + this->sp() * cos(this->h()));
 661         this->y(this->y() + this->sp() * sin(this->h()));
 662         this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
 663 }