incl/bcar.hh

   1 /*
   2  * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
   3  *
   4  * SPDX-License-Identifier: GPL-3.0-only
   5  */
   6
   7 /*! \file */
   8 #ifndef BCAR_BCAR_H
   9 #define BCAR_BCAR_H
  10
  11 #include <ostream>
  12 #include <vector>
  13
  14 namespace bcar {
  15
  16 template <typename T> int sgn(T val) {
  17         return (T(0) < val) - (val < T(0));
  18 }
  19
  20 class Line;
  21
  22 class Point {
  23 private:
  24         double _x = 0.0;
  25         double _y = 0.0;
  26 public:
  27         Point();
  28         Point(double x, double y);
  29
  30         /*! Get horizontal coordinate. */
  31         double x() const;
  32
  33         /*! Set horizontal coordinate. */
  34         void x(double x);
  35
  36         /*! Get vertical coordinate. */
  37         double y() const;
  38
  39         /*! Set vertical coordinate. */
  40         void y(double y);
  41
  42         /*! \brief Return the smallest angle between three points.
  43
  44         \see https://math.stackexchange.com/questions/361412/finding-the-angle-between-three-points
  45         */
  46         double min_angle_between(Point const& p1, Point const& p2) const;
  47
  48         /*! \brief Return `true` if `this` point is inside of polygon `poly`.
  49          *
  50          * The polygon is given by the vector of `Point`s.
  51          *
  52          * \see https://en.wikipedia.org/wiki/Even%E2%80%93odd_rule
  53          *
  54          * \param poly Polygon to consider.
  55          */
  56         bool inside_of(std::vector<Point> const& poly) const;
  57
  58         /*! \brief Return `true` if `this` point is inside the circle `c`, `r`.
  59          *
  60          * \see * https://math.stackexchange.com/questions/198764/how-to-know-if-a-point-is-inside-a-circle#198769
  61          */
  62         bool inside_of(Point const& c, double const r) const;
  63
  64         /*! \brief Return `true` if on the right side of the plane.
  65          *
  66          * The plane is given by the line `li`, where `li->b()` is the base
  67          * point and the direction is given by `li->e() - li->b()`.
  68          *
  69          * \param li The plane to consider is given by `li`.
  70          */
  71         bool on_right_side_of(Line const& li) const;
  72
  73         /*! \brief Translate self.
  74          *
  75          * \param p `Point` offset to translate by.
  76          */
  77         void translate(Point const& p);
  78
  79         /*! \brief Rotate self around the point.
  80
  81         \param c Rotation center `Point`.
  82         \param angl Angle of rotation.
  83         */
  84         void rotate(Point const& c, double const angl);
  85
  86         /*! \brief Compute reflection of `this` around the `Line`.
  87          *
  88          * \param li The plane to reflect around is given by `li`.
  89          */
  90         void reflect(Line const& li);
  91
  92         /*! Return Euclidean distance to `p`. */
  93         double edist(Point const& p) const;
  94
  95         /*! Generate output for plotting with gnuplot. */
  96         void gen_gnuplot_to(std::ostream& out);
  97
  98         bool operator==(Point const& p);
  99         friend std::ostream& operator<<(std::ostream& out, Point const& p);
 100 };
 101
 102 class Line {
 103 private:
 104         Point _b;
 105         Point _e;
 106         Point _i1;
 107         Point _i2;
 108 public:
 109         Line(Point const& fp, Point const& lp);
 110
 111         /*! Get beginning point. */
 112         Point b() const&;
 113
 114         /*! Get end point. */
 115         Point e() const&;
 116
 117         /*! Get middle point. */
 118         Point m() const;
 119
 120         /*! Get intersection point. */
 121         Point i1() const&;
 122
 123         /*! Get intersection point. */
 124         Point i2() const&;
 125
 126         /*! \brief Return if `this` line intersects with line `li`.
 127          *
 128          * If the method returns `true`, the intersection `Point` is available
 129          * in `this->i1()`.
 130          *
 131          * \see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
 132          *
 133          * \param li The line to check the intersection with.
 134          */
 135         bool intersects_with(Line const& li);
 136
 137         /*! \brief Return intersections of `this` (infinite) line and circle.
 138          *
 139          * If the method returns `true`, the intersection `Point`s are available
 140          * in `this->i1()` and `this->i2()`.
 141          *
 142          * \see https://mathworld.wolfram.com/Circle-LineIntersection.html
 143          *
 144          * \param c Circle center.
 145          * \param r Circle radius.
 146          */
 147         bool intersects_with(Point const& c, double const r);
 148
 149         double len() const;
 150
 151         double h() const;
 152
 153         /*! Generate output for plotting with gnuplot. */
 154         void gen_gnuplot_to(std::ostream& out);
 155
 156         friend std::ostream& operator<<(std::ostream& out, Line const& li);
 157 };
 158
 159 /*! Store coordinates `x`, `y`, and heading `h`. */
 160 class Pose : public virtual Point {
 161 private:
 162         double _h = 0.0;
 163 public:
 164         using Point::Point;
 165         Pose(double x, double y, double h);
 166
 167         /*! Get heading in the interval [-pi, +pi] radians. */
 168         double h() const;
 169
 170         /*! Set heading in radians. It's recomputed to [-pi, +pi]. */
 171         void h(double h);
 172
 173         /*! Set pose (`x`, `y`, and `h`.) */
 174         void set_pose(Pose const& p);
 175
 176         void rotate(Point const& c, double const angl);
 177
 178         void reflect(Line const& li);
 179
 180         bool operator==(Pose const& p);
 181         friend std::ostream& operator<<(std::ostream& out, Pose const& p);
 182 };
 183
 184 class PoseRange : public virtual Pose {
 185 private:
 186         Pose _bp;
 187         Pose _ep;
 188         void set_xyh();
 189 public:
 190         PoseRange(Pose bp, Pose ep);
 191         PoseRange(double x, double y, double b, double e);
 192
 193         Pose bp() const;
 194         Pose ep() const;
 195
 196         /*! Get heading's begin in the interval [-pi, +pi] radians. */
 197         double b() const;
 198
 199         /*! Get heading's end in the interval [-pi, +pi] radians. */
 200         double e() const;
 201
 202         void translate(Point const& p);
 203         void rotate(Point const& c, double const angl);
 204         void reflect(Line const& li);
 205
 206         friend std::ostream& operator<<(std::ostream& out, PoseRange const& p);
 207 };
 208
 209 /*! \brief Store car size.
 210  *
 211  * - The default is Renault ZOE (www.car.info)
 212  */
 213 class CarSize {
 214 private:
 215         double _curb_to_curb = 10.802166641822163;
 216         double _width_with_mirrors = 1.945;
 217         double _width = 1.771;
 218         double _wheelbase = 2.588;
 219         double _distance_to_front = 3.427;
 220         double _length = 4.084;
 221         double _front_track = 1.511;
 222 public:
 223         /*! Get curb-to-curb distance. */
 224         double ctc() const;
 225
 226         /*! Set curb-to-curb distance. */
 227         void ctc(double ctc);
 228
 229         /*! Get wheelbase. */
 230         double wb() const;
 231
 232         /*! Set wheelbase. */
 233         void wb(double wb);
 234
 235         /*! Get width. */
 236         double w() const;
 237
 238         /*! Set width. */
 239         void w(double w);
 240
 241         /*! Get width with mirrors. */
 242         double wwm() const;
 243
 244         /*! Set width with mirrors. */
 245         void wwm(double w);
 246
 247         /*! Get length. */
 248         double len() const;
 249
 250         /*! Set length. */
 251         void len(double len);
 252
 253         /*! Get distance from rear axle to front. */
 254         double df() const;
 255
 256         /*! Set distance from rear axle to front. */
 257         void df(double df);
 258
 259         /*! Get distance from rear axle to rear. */
 260         double dr() const;
 261
 262         /*! Set front track. */
 263         void ft(double ft);
 264
 265         /*! Get front track. */
 266         double ft() const;
 267
 268         /*! \brief Get minimum turning radius.
 269          *
 270          * Please, note that the method returns really _minimum turning radius_,
 271          * which is the distance from the rear axle center to the center of
 272          * left or right rotation given by the kinematics constrants, i.e.
 273          * _wheelbase and _curb-to-curb_ distance.
 274          *
 275          * Sometimes _minimum turning _radius is not radius, not minimum, or not
 276          * turning. In this method, _minimum turning _radius is minimum turning
 277          * radius.
 278          */
 279         double mtr() const;
 280
 281         /*! \brief Return inner radius.
 282          *
 283          * The inner radius is the distance from minimum turning radius circle
 284          * center to the nearest point on the car. In this case, the nearest
 285          * points on the car are rear axle endpoints.
 286          */
 287         double iradi() const;
 288
 289         /*! \brief Return outer front radius.
 290          *
 291          * The outer front radius is the distance from minimum turning radius
 292          * circle center to the farthest point on the front (from the rear axle
 293          * view) part of the car.
 294          */
 295         double ofradi() const;
 296
 297         /*! \brief Return outer rear radius.
 298          *
 299          * The outer rear radius is the distance from minimum turning radius
 300          * circle center to the farthest point on the rear (from the rear axle
 301          * view) part of the car.
 302          */
 303         double orradi() const;
 304
 305         /*! \brief Return inner mirror radius.
 306          *
 307          * The inner mirror radius is the distance from minimum turning radius
 308          * circle center to the farther mirror (from the rear axle view).
 309          */
 310         double imradi() const;
 311
 312         /*! \brief Return outer mirror radius.
 313          *
 314          * The outer mirror radius is the distance from minimum turning radius
 315          * circle center to the farther mirror (from the rear axle view).
 316          */
 317         double omradi() const;
 318
 319         /*! \brief Return length of perfect parking slot.
 320          *
 321          * The width of the slot is the same as the width of the car.
 322          *
 323          * \see Simon R. Blackburn *The Geometry of Perfect Parking*
 324          * \see https://www.ma.rhul.ac.uk/SRBparking
 325          */
 326         double perfect_parking_slot_len() const;
 327 };
 328
 329 /*! Store car motion. */
 330 class CarMove {
 331 private:
 332         double _speed = 0.0;
 333         double _steer = 0.0;
 334 public:
 335         /*! Get speed. */
 336         double sp() const;
 337
 338         /*! Set speed. */
 339         void sp(double sp);
 340
 341         /*! Get steer. */
 342         double st() const;
 343
 344         /*! Set steer. */
 345         void st(double st);
 346 };
 347
 348 /*! \brief Geometrical computations of a bicycle car.
 349  *
 350  * - `x()` and `y()` methods returns coordinates of rear axle center.
 351  */
 352 class BicycleCar : public virtual Pose, public virtual CarSize,
 353                 public virtual CarMove {
 354 private:
 355 public:
 356         /*! \brief Return `true` if `this` can drive to `p` trivially.
 357          *
 358          * Trivially means that `this` can drive to `p` by line segment - circle
 359          * arc - line segment.
 360          *
 361          * \param p `PoseRange` (resp. `Pose`) to achieve.
 362          */
 363         bool drivable(PoseRange const& p) const;
 364         bool drivable(Pose const& p) const;
 365
 366         /*! Set maximum steering angle. */
 367         void set_max_steer();
 368
 369         /*! Get frame's left front x coordinate. */
 370         double lfx() const;
 371
 372         /*! Get frame's left front y coordinate. */
 373         double lfy() const;
 374
 375         /*! Get frame's left rear x coordinate. */
 376         double lrx() const;
 377
 378         /*! Get frame's left rear y coordinate. */
 379         double lry() const;
 380
 381         /*! Get frame's right rear x coordinate. */
 382         double rrx() const;
 383
 384         /*! Get frame's right rear y coordinate. */
 385         double rry() const;
 386
 387         /*! Get frame's right front x coordinate. */
 388         double rfx() const;
 389
 390         /*! Get frame's right front y coordinate. */
 391         double rfy() const;
 392
 393         /*! Get frame's left front point. */
 394         Point lf() const;
 395
 396         /*! Get frame's left rear point. */
 397         Point lr() const;
 398
 399         /*! Get frame's right rear point. */
 400         Point rr() const;
 401
 402         /*! Get frame's right front point. */
 403         Point rf() const;
 404
 405         /*! Get frame's left side. */
 406         Line left() const;
 407
 408         /*! Get frame's rear side. */
 409         Line rear() const;
 410
 411         /*! Get frame's right side. */
 412         Line right() const;
 413
 414         /*! Get frame's front side. */
 415         Line front() const;
 416
 417         /*! Get frame's left rear axle x coordinate. */
 418         double lrax() const;
 419
 420         /*! Get frame's left rear axle y coordinate. */
 421         double lray() const;
 422
 423         /*! Get frame's right rear axle x coordinate. */
 424         double rrax() const;
 425
 426         /*! Get frame's right rear axle y coordinate. */
 427         double rray() const;
 428
 429         /*! Get frame's left rear axle point. */
 430         Point lra() const;
 431
 432         /*! Get frame's right rear axle point. */
 433         Point rra() const;
 434
 435         /*! Get frame's left front axle x coordinate. */
 436         double lfax() const;
 437
 438         /*! Get frame's left front axle y coordinate. */
 439         double lfay() const;
 440
 441         /*! Get frame's right front axle x coordinate. */
 442         double rfax() const;
 443
 444         /*! Get frame's right front axle y coordinate. */
 445         double rfay() const;
 446
 447         /*! Get iframe's left front axle point. */
 448         Point lfa() const;
 449
 450         /*! Get frame's right front axle point. */
 451         Point rfa() const;
 452
 453         /*! Get frame's left front mirror x coordinate. */
 454         double lfmx() const;
 455
 456         /*! Get frame's left front mirror y coordinate. */
 457         double lfmy() const;
 458
 459         /*! Get frame's right front mirror x coordinate. */
 460         double rfmx() const;
 461
 462         /*! Get frame's right front mirror y coordinate. */
 463         double rfmy() const;
 464
 465         /*! Get iframe's left front mirror point. */
 466         Point lfm() const;
 467
 468         /*! Get frame's right front mirror point. */
 469         Point rfm() const;
 470
 471         /*! Get frame's center front x coordinate. */
 472         double cfx() const;
 473
 474         /*! Get frame's center front y coordinate. */
 475         double cfy() const;
 476
 477         /*! Get frame's center front point. */
 478         Point cf() const;
 479
 480         /*! Min. turning radius circle center on left. */
 481         Point ccl() const;
 482
 483         /*! Min. turning radius circle center on rigth. */
 484         Point ccr() const;
 485
 486         /*! Next car position based on speed `sp` and steer `st`. */
 487         void next();
 488
 489         /*! Options for generating output for gnuplot. */
 490         class GenPlotOpts {
 491         public:
 492                 bool LF_POINT = false;
 493                 bool LR_POINT = false;
 494                 bool RR_POINT = false;
 495                 bool RF_POINT = false;
 496                 bool LFM_POINT = false;
 497                 bool RFM_POINT = false;
 498                 bool CRA_POINT = false;
 499                 bool CAR_POINT = false;
 500                 bool LRA_POINT = false;
 501                 bool RRA_POINT = false;
 502
 503                 bool LEFT = false;
 504                 bool RIGHT = false;
 505                 bool REAR = false;
 506                 bool FRONT = false;
 507                 bool FRAME = false; // LEFT, RIGHT, REAR, FRONT
 508                 bool ARROW = false;
 509                 bool CROSS = false;
 510                 bool CAR = false; // CROSS, ARROW, FRAME
 511                 bool LEFT_MIRROR = false;
 512                 bool RIGHT_MIRROR = false;
 513                 bool MIRRORS = false; // RIGHT_MIRROR, LEFT_MIRROR
 514                 bool ALL = true; // MIRRORS, CAR
 515         };
 516
 517         /*! Generate output for plotting with gnuplot. */
 518         void gen_gnuplot_to(std::ostream& out, GenPlotOpts opts);
 519 };
 520
 521 } // namespace bcar
 522 #endif /* BCAR_BCAR_H */