api/bcar.h

   1 #ifndef BCAR_H
   2 #define BCAR_H
   3
   4 #include <ostream>
   5 #include <tuple>
   6 #include <vector>
   7
   8 /*! \brief Bicycle car basic class.
   9
  10 This class contains some geometrical computations of bicycle car.
  11
  12 \param x Horizontal coordinate of rear axle center.
  13 \param y Vertical coordinate of rear axle center.
  14 \param h Heading of the car.
  15 \param mtr Minimum turning radius.
  16 \param wb Wheelbase.
  17 \param w The width of the car.
  18 \param l The length of the car.
  19 \param he The height of the car.
  20 \param sd The safety distance.
  21 \param df Distance from rear axle center to the front of the car.
  22 \param dr Distance from rear axle center to the back of the car.
  23 \param sp Speed of the car.
  24 \param st Steering of the car.
  25 */
  26 class BicycleCar {
  27         private:
  28                 // coordinates
  29                 double x_ = 0;
  30                 double y_ = 0;
  31                 double h_ = 0;
  32                 // kinematic constraints
  33                 double mtr_ = 10.820;
  34                 double wb_ = 2.450;
  35                 // dimensions
  36                 double w_ = 1.625;
  37                 double l_ = 3.760;
  38                 double he_ = 1.450;
  39                 double sd_ = 0;
  40                 double df_ = 3.105;
  41                 double dr_ = 0.655;
  42                 // moving
  43                 double sp_ = 0;
  44                 double st_ = 0;
  45         public:
  46                 // kinematic constraints
  47                 /*! \brief Return `false` if `bc` is not achievable.
  48
  49                 When `true` is returned the `bc` may still be not
  50                 achievable because in the current implementation,
  51                 heading is not considered.
  52
  53                 \param[in] bc The bicycle car to achieve.
  54                 */
  55                 bool drivable(const BicycleCar &bc) const;
  56                 /*! \brief Return inner radius.
  57
  58                 The inner radius is the distance from minimum turning
  59                 radius circle center to the nearest point on the car. In
  60                 this case, the nearest points on the car are rear axle
  61                 endpoints.
  62                 */
  63                 double iradi() const;
  64                 /*! \brief Return outer front radius.
  65
  66                 The outer front radius is the distance from minimum
  67                 turning radius circle center to the farthest point on
  68                 the front (from the rear axle view) part of the car.
  69                 */
  70                 double ofradi() const;
  71                 /*! \brief Return outer rear radius.
  72
  73                 The outer rear radius is the distance from minimum
  74                 turning radius circle center to the farthest point on
  75                 the rear (from the rear axle view) part of the car.
  76                 */
  77                 double orradi() const;
  78
  79                 // car frame
  80                 double lfx() const; double lfy() const;
  81                 double lrx() const; double lry() const;
  82                 double rrx() const; double rry() const;
  83                 double rfx() const; double rfy() const;
  84
  85                 double ralx() const; double raly() const;
  86                 double rarx() const; double rary() const;
  87
  88                 /*! \brief Min. turning radius circle center on left.
  89
  90                 Important are coordinates `x` and `y`. The heading `h`
  91                 is set as the heading of `this->h()`.
  92                 */
  93                 BicycleCar ccl() const;
  94                 /*! \brief Min. turning radius circle center on rigth.
  95
  96                 Important are coordinates `x` and `y`. The heading `h`
  97                 is set as the heading of `this->h()`.
  98                 */
  99                 BicycleCar ccr() const;
 100
 101                 // moving
 102                 /*! \brief Next car position based on `sp` and `st`.
 103
 104                 Where `sp` is speed and `st` is steering of the car.
 105                 */
 106                 void next();
 107
 108                 // getters, setters
 109                 double x() const { return this->x_; }
 110                 void x(double x) { this->x_ = x; }
 111
 112                 double y() const { return this->y_; }
 113                 void y(double y) { this->y_ = y; }
 114
 115                 double h() const { return this->h_; }
 116                 void h(double h) { this->h_ = h; }
 117
 118                 double mtr() const { return this->mtr_; }
 119                 void mtr(double mtr) { this->mtr_ = mtr; }
 120
 121                 double wb() const { return this->wb_; }
 122                 void wb(double wb) { this->wb_ = wb; }
 123
 124                 double w() const { return this->w_; }
 125                 void w(double w) { this->w_ = w; }
 126
 127                 double l() const { return this->l_; }
 128                 void l(double l) { this->l_ = l; }
 129
 130                 double he() const { return this->he_; }
 131                 void he(double he) { this->he_ = he; }
 132
 133                 double sd() const { return this->sd_; }
 134                 void sd(double sd) { this->sd_ = sd; }
 135
 136                 double df() const { return this->df_; }
 137                 void df(double df) { this->df_ = df; }
 138
 139                 double dr() const { return this->dr_; }
 140                 void dr(double dr) { this->dr_ = dr; }
 141
 142                 double sp() const { return this->sp_; }
 143                 void sp(double sp) { this->sp_ = sp; }
 144
 145                 double st() const { return this->st_; }
 146                 void st(double st) { this->st_ = st; }
 147
 148                 BicycleCar();
 149                 friend std::ostream &operator<<(
 150                         std::ostream &out,
 151                         const BicycleCar &bc
 152                 )
 153                 {
 154                         out << "[" << bc.x();
 155                         out << "," << bc.y();
 156                         out << "," << bc.h();
 157                         out << "]";
 158                         return out;
 159                 }
 160 };
 161
 162 /*! \brief Is `x, y` coordinate in polynom `poly`?
 163
 164 Return `true` if `x, y` coordinate is inside of polynom `poly`.
 165
 166 \see https://en.wikipedia.org/wiki/Even%E2%80%93odd_rule
 167
 168 \param x Horizontal coordinate.
 169 \param y Vertical coordinate.
 170 \param poly The vector of coordinates.
 171 */
 172 bool inside(double x, double y, std::vector<std::tuple<double, double>> poly);
 173
 174 /*! \brief Return intersection of two line segments.
 175
 176 The output is tuple `std::tuple<bool, double, double>`, where the first
 177 value is true when there is an intersection and false otherwise. The
 178 second and third parameters in the return tuple are coordinates of the
 179 intersection.
 180
 181 \see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
 182
 183 \param x1 First line segment first `x` coordinate.
 184 \param y1 First line segment first `y` coordinate.
 185 \param x2 First line segment second `x` coordinate.
 186 \param y2 First line segment second `y` coordinate.
 187 \param x3 Second line segment first `x` coordinate.
 188 \param y3 Second line segment first `y` coordinate.
 189 \param x4 Second line segment second `x` coordinate.
 190 \param y4 Second line segment second `y` coordinate.
 191 */
 192 std::tuple<bool, double, double> intersect(
 193         double x1, double y1,
 194         double x2, double y2,
 195         double x3, double y3,
 196         double x4, double y4
 197 );
 198
 199 #endif /* BCAR_H */