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