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