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