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