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