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