incl/bcar.hh

   1 /*! \file */
   2 #ifndef BCAR_BCAR_H
   3 #define BCAR_BCAR_H
   4
   5 #include <ostream>
   6 #include <vector>
   7
   8 namespace bcar {
   9
  10 template <typename T> int sgn(T val) {
  11         return (T(0) < val) - (val < T(0));
  12 }
  13
  14 class Line;
  15
  16 class Point {
  17 private:
  18         double x_ = 0.0;
  19         double y_ = 0.0;
  20 public:
  21         Point(double x, double y);
  22         Point();
  23
  24         /*! Get horizontal coordinate. */
  25         double x() const;
  26
  27         /*! Set horizontal coordinate. */
  28         void x(double x);
  29
  30         /*! Get vertical coordinate. */
  31         double y() const;
  32
  33         /*! Set vertical coordinate. */
  34         void y(double y);
  35
  36         /*! \brief Return the smallest angle between three points.
  37
  38         \see https://math.stackexchange.com/questions/361412/finding-the-angle-between-three-points
  39         */
  40         double min_angle_between(Point const& p1, Point const& p2) const;
  41
  42         /*! \brief Return `true` if `this` point is inside of polygon `poly`.
  43          *
  44          * The polygon is given by the vector of `Point`s.
  45          *
  46          * \see https://en.wikipedia.org/wiki/Even%E2%80%93odd_rule
  47          *
  48          * \param poly Polygon to consider.
  49          */
  50         bool inside_of(std::vector<Point> const& poly) const;
  51
  52         /*! \brief Return `true` if on the right side of the plane.
  53          *
  54          * The plane is given by the line `li`, where `li->fp()` is the base
  55          * point and the direction is given by `li->lp() - li->fp()`.
  56          *
  57          * \param li The plane to consider is given by `li`.
  58          */
  59         bool on_right_side_of(Line const& li) const;
  60
  61         /*! \brief Rotate self around the point.
  62
  63         \param c Rotation center `Point`.
  64         \param angl Angle of rotation.
  65         */
  66         void rotate(Point const& c, double const angl);
  67
  68         friend std::ostream& operator<<(std::ostream& out, Point const& p);
  69 };
  70
  71 class Line {
  72 private:
  73         Point first;
  74         Point last;
  75         Point intersection1;
  76         Point intersection2;
  77 public:
  78         Line(Point const& fp, Point const& lp);
  79
  80         /*! Get first point. */
  81         Point fp() const&;
  82
  83         /*! Get last point. */
  84         Point lp() const&;
  85
  86         /*! Get intersection point. */
  87         Point in1() const&;
  88
  89         /*! Get intersection point. */
  90         Point in2() const&;
  91
  92         /*! \brief Return if `this` line intersects with line `li`.
  93          *
  94          * If the method returns `true`, the intersection `Point` is available
  95          * in `this->in1()`.
  96          *
  97          * \see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
  98          *
  99          * \param li The line to check the intersection with.
 100          */
 101         bool intersects_with(Line const& li);
 102
 103         /*! \brief Return intersections of `this` (infinite) line and circle.
 104          *
 105          * If the method returns `true`, the intersection `Point`s are available
 106          * in `this->in1()` and `this->in2()`.
 107          *
 108          * \see https://mathworld.wolfram.com/Circle-LineIntersection.html
 109          *
 110          * \param c Circle center.
 111          * \param r Circle radius.
 112          */
 113         bool intersects_with(Point const& c, double const r);
 114
 115         double len() const;
 116 };
 117
 118 /*! Store coordinates `x`, `y`, and heading `h`. */
 119 class Pose : public virtual Point {
 120 private:
 121         double h_ = 0.0;
 122 public:
 123         Pose(double x, double y, double h);
 124         Pose();
 125
 126         /*! Get heading in the interval [-pi, +pi] radians. */
 127         double h() const;
 128
 129         /*! Set heading in radians. It's recomputed to [-pi, +pi]. */
 130         void h(double h);
 131
 132         /*! Set pose (`x`, `y`, and `h`.) */
 133         void set_pose(Pose const& p);
 134
 135         void rotate(Point const& c, double const angl);
 136
 137         friend std::ostream& operator<<(std::ostream& out, Pose const& p);
 138 };
 139
 140 class PoseRange : public virtual Pose {
 141 private:
 142         double e_ = 0.0;
 143         using Pose::h;
 144 public:
 145         /*! Get heading's begin in the interval [-pi, +pi] radians. */
 146         double b() const;
 147
 148         /*! Set heading's begin in radians. It's recomputed to [-pi, +pi]. */
 149         void b(double b);
 150
 151         /*! Get heading's end in the interval [-pi, +pi] radians. */
 152         double e() const;
 153
 154         /*! Set heading's end in radians. It's recomputed to [-pi, +pi]. */
 155         void e(double e);
 156
 157         void rotate(Point const& c, double const angl);
 158
 159         friend std::ostream& operator<<(std::ostream& out, PoseRange const& p);
 160 };
 161
 162 /*! \brief Store car size.
 163  *
 164  * - Default is https://en.wikipedia.org/wiki/Fiat_Punto
 165  */
 166 class CarSize {
 167 private:
 168         double curb_to_curb = 10.820;
 169         double width = 1.625;
 170         double wheelbase = 2.450;
 171         double distance_to_front = 3.105;
 172         double length = 3.760;
 173 public:
 174         /*! Get curb-to-curb distance. */
 175         double ctc() const;
 176
 177         /*! Set curb-to-curb distance. */
 178         void ctc(double ctc);
 179
 180         /*! Get wheelbase. */
 181         double wb() const;
 182
 183         /*! Set wheelbase. */
 184         void wb(double wb);
 185
 186         /*! Get width. */
 187         double w() const;
 188
 189         /*! Set width. */
 190         void w(double w);
 191
 192         /*! Get length. */
 193         double len() const;
 194
 195         /*! Set length. */
 196         void len(double len);
 197
 198         /*! Get distance from rear axle to front. */
 199         double df() const;
 200
 201         /*! Set distance from rear axle to front. */
 202         void df(double df);
 203
 204         /*! Get distance from rear axle to rear. */
 205         double dr() const;
 206
 207         /*! \brief Get minimum turning radius.
 208          *
 209          * Please, note that the method returns really _minimum turning radius_,
 210          * which is the distance from the reare axle center to the center of
 211          * left or right rotation given by the kinematics constrants, i.e.
 212          * _wheelbase_ and _curb-to-curb_ distance.
 213          *
 214          * Sometimes _minimum turning radius_ is not radius, not minimum, or not
 215          * turning. In this method, _minimum turning radius_ is minimum turning
 216          * radius.
 217          */
 218         double mtr() const;
 219
 220         /*! \brief Return inner radius.
 221          *
 222          * The inner radius is the distance from minimum turning radius circle
 223          * center to the nearest point on the car. In this case, the nearest
 224          * points on the car are rear axle endpoints.
 225          */
 226         double iradi() const;
 227
 228         /*! \brief Return outer front radius.
 229          *
 230          * The outer front radius is the distance from minimum turning radius
 231          * circle center to the farthest point on the front (from the rear axle
 232          * view) part of the car.
 233          */
 234         double ofradi() const;
 235
 236         /*! \brief Return outer rear radius.
 237          *
 238          * The outer rear radius is the distance from minimum turning radius
 239          * circle center to the farthest point on the rear (from the rear axle
 240          * view) part of the car.
 241          */
 242         double orradi() const;
 243
 244         /*! \brief Return length of perfect parking slot.
 245          *
 246          * The width of the slot is the same as the width of the car.
 247          *
 248          * \see Simon R. Blackburn *The Geometry of Perfect Parking*
 249          * \see https://www.ma.rhul.ac.uk/SRBparking
 250          */
 251         double perfect_parking_slot_len() const;
 252 };
 253
 254 /*! Store car motion. */
 255 class CarMove {
 256 private:
 257         double speed = 0.0;
 258         double steer = 0.0;
 259 public:
 260         /*! Get speed. */
 261         double sp() const;
 262
 263         /*! Set speed. */
 264         void sp(double sp);
 265
 266         /*! Get steer. */
 267         double st() const;
 268
 269         /*! Set steer. */
 270         void st(double st);
 271 };
 272
 273 /*! \brief Geometrical computations of a bicycle car.
 274  *
 275  * - `x()` and `y()` methods returns coordinates of rear axle center.
 276  */
 277 class BicycleCar : public virtual Pose, public virtual CarSize,
 278                 public virtual CarMove {
 279 private:
 280 public:
 281         /*! \brief Return `false` if `bc` is not achievable.
 282          *
 283          * When `false` is returned the `bc` may still be drivable, but not
 284          * trivially, i.e. by "line segment - circle arc - line segment".
 285          *
 286          * \param p `PoseRange` (resp. `Pose`) to achieve.
 287          */
 288         bool drivable(PoseRange const& p) const;
 289         bool drivable(Pose const& p) const;
 290
 291         /*! Set maximum steering angle. */
 292         void set_max_steer();
 293
 294         /*! Get frame's left front x coordinate. */
 295         double lfx() const;
 296
 297         /*! Get frame's left front y coordinate. */
 298         double lfy() const;
 299
 300         /*! Get frame's left rear x coordinate. */
 301         double lrx() const;
 302
 303         /*! Get frame's left rear y coordinate. */
 304         double lry() const;
 305
 306         /*! Get frame's right rear x coordinate. */
 307         double rrx() const;
 308
 309         /*! Get frame's right rear y coordinate. */
 310         double rry() const;
 311
 312         /*! Get frame's right front x coordinate. */
 313         double rfx() const;
 314
 315         /*! Get frame's right front y coordinate. */
 316         double rfy() const;
 317
 318         /*! Get rear axle's left x coordinate. */
 319         double ralx() const;
 320
 321         /*! Get rear axle's left y coordinate. */
 322         double raly() const;
 323
 324         /*! Get rear axle's right x coordinate. */
 325         double rarx() const;
 326
 327         /*! Get rear axle's right y coordinate. */
 328         double rary() const;
 329
 330         /*! Min. turning radius circle center on left. */
 331         Point ccl() const;
 332
 333         /*! Min. turning radius circle center on rigth. */
 334         Point ccr() const;
 335
 336         /*! Next car position based on speed `sp` and steer `st`. */
 337         void next();
 338 };
 339
 340 } // namespace bcar
 341 #endif /* BCAR_BCAR_H */