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