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         bool operator==(Point const& p);
  90         friend std::ostream& operator<<(std::ostream& out, Point const& p);
  91 };
  92
  93 class Line {
  94 private:
  95         Point b_;
  96         Point e_;
  97         Point i1_;
  98         Point i2_;
  99 public:
 100         Line(Point const& fp, Point const& lp);
 101
 102         /*! Get beginning point. */
 103         Point b() const&;
 104
 105         /*! Get end point. */
 106         Point e() const&;
 107
 108         /*! Get middle point. */
 109         Point m() const;
 110
 111         /*! Get intersection point. */
 112         Point i1() const&;
 113
 114         /*! Get intersection point. */
 115         Point i2() const&;
 116
 117         /*! \brief Return if `this` line intersects with line `li`.
 118          *
 119          * If the method returns `true`, the intersection `Point` is available
 120          * in `this->i1()`.
 121          *
 122          * \see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
 123          *
 124          * \param li The line to check the intersection with.
 125          */
 126         bool intersects_with(Line const& li);
 127
 128         /*! \brief Return intersections of `this` (infinite) line and circle.
 129          *
 130          * If the method returns `true`, the intersection `Point`s are available
 131          * in `this->i1()` and `this->i2()`.
 132          *
 133          * \see https://mathworld.wolfram.com/Circle-LineIntersection.html
 134          *
 135          * \param c Circle center.
 136          * \param r Circle radius.
 137          */
 138         bool intersects_with(Point const& c, double const r);
 139
 140         double len() const;
 141
 142         double h() const;
 143
 144         friend std::ostream& operator<<(std::ostream& out, Line const& li);
 145 };
 146
 147 /*! Store coordinates `x`, `y`, and heading `h`. */
 148 class Pose : public virtual Point {
 149 private:
 150         double h_ = 0.0;
 151 public:
 152         using Point::Point;
 153         Pose(double x, double y, double h);
 154
 155         /*! Get heading in the interval [-pi, +pi] radians. */
 156         double h() const;
 157
 158         /*! Set heading in radians. It's recomputed to [-pi, +pi]. */
 159         void h(double h);
 160
 161         /*! Set pose (`x`, `y`, and `h`.) */
 162         void set_pose(Pose const& p);
 163
 164         void rotate(Point const& c, double const angl);
 165
 166         void reflect(Line const& li);
 167
 168         bool operator==(Pose const& p);
 169         friend std::ostream& operator<<(std::ostream& out, Pose const& p);
 170 };
 171
 172 class PoseRange : public virtual Pose {
 173 private:
 174         Pose bp_;
 175         Pose ep_;
 176         void set_xyh();
 177 public:
 178         PoseRange(Pose bp, Pose ep);
 179         PoseRange(double x, double y, double b, double e);
 180
 181         Pose bp() const;
 182         Pose ep() const;
 183
 184         /*! Get heading's begin in the interval [-pi, +pi] radians. */
 185         double b() const;
 186
 187         /*! Get heading's end in the interval [-pi, +pi] radians. */
 188         double e() const;
 189
 190         void translate(Point const& p);
 191         void rotate(Point const& c, double const angl);
 192         void reflect(Line const& li);
 193
 194         friend std::ostream& operator<<(std::ostream& out, PoseRange const& p);
 195 };
 196
 197 /*! \brief Store car size.
 198  *
 199  * - Default is https://en.wikipedia.org/wiki/Fiat_Punto
 200  */
 201 class CarSize {
 202 private:
 203         double curb_to_curb_ = 10.820;
 204         double width_ = 1.625;
 205         double wheelbase_ = 2.450;
 206         double distance_to_front_ = 3.105;
 207         double length_ = 3.760;
 208 public:
 209         /*! Get curb-to-curb distance. */
 210         double ctc() const;
 211
 212         /*! Set curb-to-curb distance. */
 213         void ctc(double ctc);
 214
 215         /*! Get wheelbase. */
 216         double wb() const;
 217
 218         /*! Set wheelbase. */
 219         void wb(double wb);
 220
 221         /*! Get width. */
 222         double w() const;
 223
 224         /*! Set width. */
 225         void w(double w);
 226
 227         /*! Get length. */
 228         double len() const;
 229
 230         /*! Set length. */
 231         void len(double len);
 232
 233         /*! Get distance from rear axle to front. */
 234         double df() const;
 235
 236         /*! Set distance from rear axle to front. */
 237         void df(double df);
 238
 239         /*! Get distance from rear axle to rear. */
 240         double dr() const;
 241
 242         /*! \brief Get minimum turning radius.
 243          *
 244          * Please, note that the method returns really _minimum turning radius_,
 245          * which is the distance from the rear axle center to the center of
 246          * left or right rotation given by the kinematics constrants, i.e.
 247          * _wheelbase_ and _curb-to-curb_ distance.
 248          *
 249          * Sometimes _minimum turning radius_ is not radius, not minimum, or not
 250          * turning. In this method, _minimum turning radius_ is minimum turning
 251          * radius.
 252          */
 253         double mtr() const;
 254
 255         /*! \brief Return inner radius.
 256          *
 257          * The inner radius is the distance from minimum turning radius circle
 258          * center to the nearest point on the car. In this case, the nearest
 259          * points on the car are rear axle endpoints.
 260          */
 261         double iradi() const;
 262
 263         /*! \brief Return outer front radius.
 264          *
 265          * The outer front radius is the distance from minimum turning radius
 266          * circle center to the farthest point on the front (from the rear axle
 267          * view) part of the car.
 268          */
 269         double ofradi() const;
 270
 271         /*! \brief Return outer rear radius.
 272          *
 273          * The outer rear radius is the distance from minimum turning radius
 274          * circle center to the farthest point on the rear (from the rear axle
 275          * view) part of the car.
 276          */
 277         double orradi() const;
 278
 279         /*! \brief Return length of perfect parking slot.
 280          *
 281          * The width of the slot is the same as the width of the car.
 282          *
 283          * \see Simon R. Blackburn *The Geometry of Perfect Parking*
 284          * \see https://www.ma.rhul.ac.uk/SRBparking
 285          */
 286         double perfect_parking_slot_len() const;
 287 };
 288
 289 /*! Store car motion. */
 290 class CarMove {
 291 private:
 292         double speed_ = 0.0;
 293         double steer_ = 0.0;
 294 public:
 295         /*! Get speed. */
 296         double sp() const;
 297
 298         /*! Set speed. */
 299         void sp(double sp);
 300
 301         /*! Get steer. */
 302         double st() const;
 303
 304         /*! Set steer. */
 305         void st(double st);
 306 };
 307
 308 /*! \brief Geometrical computations of a bicycle car.
 309  *
 310  * - `x()` and `y()` methods returns coordinates of rear axle center.
 311  */
 312 class BicycleCar : public virtual Pose, public virtual CarSize,
 313                 public virtual CarMove {
 314 private:
 315 public:
 316         /*! \brief Return `true` if `this` can drive to `p` trivially.
 317          *
 318          * Trivially means that `this` can drive to `p` by line segment - circle
 319          * arc - line segment.
 320          *
 321          * \param p `PoseRange` (resp. `Pose`) to achieve.
 322          */
 323         bool drivable(PoseRange const& p) const;
 324         bool drivable(Pose const& p) const;
 325
 326         /*! Set maximum steering angle. */
 327         void set_max_steer();
 328
 329         /*! Get frame's left front x coordinate. */
 330         double lfx() const;
 331
 332         /*! Get frame's left front y coordinate. */
 333         double lfy() const;
 334
 335         /*! Get frame's left rear x coordinate. */
 336         double lrx() const;
 337
 338         /*! Get frame's left rear y coordinate. */
 339         double lry() const;
 340
 341         /*! Get frame's right rear x coordinate. */
 342         double rrx() const;
 343
 344         /*! Get frame's right rear y coordinate. */
 345         double rry() const;
 346
 347         /*! Get frame's right front x coordinate. */
 348         double rfx() const;
 349
 350         /*! Get frame's right front y coordinate. */
 351         double rfy() const;
 352
 353         /*! Get frame's left front point. */
 354         Point lf() const;
 355
 356         /*! Get frame's left rear point. */
 357         Point lr() const;
 358
 359         /*! Get frame's right rear point. */
 360         Point rr() const;
 361
 362         /*! Get frame's right front point. */
 363         Point rf() const;
 364
 365         /*! Get frame's left side. */
 366         Line left() const;
 367
 368         /*! Get frame's rear side. */
 369         Line rear() const;
 370
 371         /*! Get frame's right side. */
 372         Line right() const;
 373
 374         /*! Get frame's front side. */
 375         Line front() const;
 376
 377         /*! Get rear axle's left x coordinate. */
 378         double ralx() const;
 379
 380         /*! Get rear axle's left y coordinate. */
 381         double raly() const;
 382
 383         /*! Get rear axle's right x coordinate. */
 384         double rarx() const;
 385
 386         /*! Get rear axle's right y coordinate. */
 387         double rary() const;
 388
 389         /*! Min. turning radius circle center on left. */
 390         Point ccl() const;
 391
 392         /*! Min. turning radius circle center on rigth. */
 393         Point ccr() const;
 394
 395         /*! Next car position based on speed `sp` and steer `st`. */
 396         void next();
 397 };
 398
 399 } // namespace bcar
 400 #endif /* BCAR_BCAR_H */