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