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 in the interval [-pi,+pi] radians.
  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 ctc_ = 10.820; // curb-to-curb
  34                 // FIXME is not mtr; curb-to-curb is 10.820
  35                 double mtr_ = 10.820;
  36                 double wb_ = 2.450;
  37                 // dimensions
  38                 double w_ = 1.625;
  39                 double l_ = 3.760;
  40                 double he_ = 1.450;
  41                 double sd_ = 0;
  42                 double df_ = 3.105;
  43                 double dr_ = 0.655;
  44                 // moving
  45                 double sp_ = 0;
  46                 double st_ = 0;
  47         public:
  48                 // kinematic constraints
  49                 /*! \brief Return `false` if `bc` is not achievable.
  50
  51                 When `false` is returned the `bc` may still be drivable,
  52                 because only "line segment - circle arc - line segment"
  53                 paths are considered in ``drivable`` method.
  54
  55                 \param[in] bc The bicycle car to achieve.
  56                 */
  57                 bool drivable(const BicycleCar &bc) const;
  58                 /*! \brief Return inner radius.
  59
  60                 The inner radius is the distance from minimum turning
  61                 radius circle center to the nearest point on the car. In
  62                 this case, the nearest points on the car are rear axle
  63                 endpoints.
  64                 */
  65                 double iradi() const;
  66                 /*! \brief Return outer front radius.
  67
  68                 The outer front radius is the distance from minimum
  69                 turning radius circle center to the farthest point on
  70                 the front (from the rear axle view) part of the car.
  71                 */
  72                 double ofradi() const;
  73                 /*! \brief Return outer rear radius.
  74
  75                 The outer rear radius is the distance from minimum
  76                 turning radius circle center to the farthest point on
  77                 the rear (from the rear axle view) part of the car.
  78                 */
  79                 double orradi() const;
  80                 /*! \brief Return length of perfect parking slot.
  81
  82                 The width of the slot is the same as the width of the
  83                 car.
  84                 */
  85                 double perfect_parking_slot_len() const;
  86                 /*! \brief Set maximum steering angle.
  87                 */
  88                 void set_max_steer();
  89
  90                 // car frame
  91                 double lfx() const; double lfy() const;
  92                 double lrx() const; double lry() const;
  93                 double rrx() const; double rry() const;
  94                 double rfx() const; double rfy() const;
  95
  96                 double ralx() const; double raly() const;
  97                 double rarx() const; double rary() const;
  98
  99                 /*! \brief Min. turning radius circle center on left.
 100
 101                 Important are coordinates `x` and `y`. The heading `h`
 102                 is set as the heading of `this->h()`.
 103                 */
 104                 BicycleCar ccl() const;
 105                 /*! \brief Min. turning radius circle center on rigth.
 106
 107                 Important are coordinates `x` and `y`. The heading `h`
 108                 is set as the heading of `this->h()`.
 109                 */
 110                 BicycleCar ccr() const;
 111
 112                 // moving
 113                 /*! \brief Next car position based on `sp` and `st`.
 114
 115                 Where `sp` is speed and `st` is steering of the car.
 116                 */
 117                 void next();
 118                 /*! \brief Rotate self around the point.
 119
 120                 \param cx Horizontal coordinate of rotation center.
 121                 \param cy Vertical coordinate of rotation center.
 122                 \param angl Angle of rotation.
 123                 */
 124                 void rotate(double cx, double cy, double angl);
 125
 126                 // getters, setters
 127                 double x() const { return this->x_; }
 128                 void x(double x) { this->x_ = x; }
 129
 130                 double y() const { return this->y_; }
 131                 void y(double y) { this->y_ = y; }
 132
 133                 double h() const { return this->h_; }
 134                 void h(double h)
 135                 {
 136                         while (h < -M_PI)
 137                                 h += 2 * M_PI;
 138                         while (h > +M_PI)
 139                                 h -= 2 * M_PI;
 140                         this->h_ = h;
 141                 }
 142
 143                 double ctc() const { return this->ctc_; }
 144                 void ctc(double ctc) { this->ctc_ = ctc; }
 145
 146                 double mtr() const { return this->mtr_; }
 147                 void mtr(double mtr) { this->mtr_ = mtr; }
 148
 149                 double wb() const { return this->wb_; }
 150                 void wb(double wb) { this->wb_ = wb; }
 151
 152                 double w() const { return this->w_; }
 153                 void w(double w) { this->w_ = w; }
 154
 155                 double l() const { return this->l_; }
 156                 void l(double l) { this->l_ = l; }
 157
 158                 double he() const { return this->he_; }
 159                 void he(double he) { this->he_ = he; }
 160
 161                 double sd() const { return this->sd_; }
 162                 void sd(double sd) { this->sd_ = sd; }
 163
 164                 double df() const { return this->df_; }
 165                 void df(double df) { this->df_ = df; }
 166
 167                 double dr() const { return this->dr_; }
 168                 void dr(double dr) { this->dr_ = dr; }
 169
 170                 double sp() const { return this->sp_; }
 171                 void sp(double sp) { this->sp_ = sp; }
 172
 173                 double st() const { return this->st_; }
 174                 void st(double st) { this->st_ = st; }
 175
 176                 BicycleCar();
 177                 friend std::ostream &operator<<(
 178                         std::ostream &out,
 179                         const BicycleCar &bc
 180                 )
 181                 {
 182                         out << "[" << bc.x();
 183                         out << "," << bc.y();
 184                         out << "," << bc.h();
 185                         out << "]";
 186                         return out;
 187                 }
 188 };
 189
 190 /*! \brief Does two polygons collide?
 191
 192 Return the tuple `std::tuple<bool, int, int>`, where the first value is
 193 `true` when there is an intersection of some segments of the polygons
 194 `p1` and `p2` and `false` otherwise. The second and third parameters in
 195 the return tuple are indexes of the first collision, where index starts
 196 at 0.
 197
 198 \param p1 The first polygon to check against collision.
 199 \param p2 The second polygon to check against collision.
 200 */
 201 std::tuple<bool, unsigned int, unsigned int> collide(
 202         std::vector<std::tuple<double, double>> &p1,
 203         std::vector<std::tuple<double, double>> &p2
 204 );
 205
 206 /*! \brief Is `x, y` coordinate in polygon `poly`?
 207
 208 Return `true` if `x, y` coordinate is inside of polygon `poly`.
 209
 210 \see https://en.wikipedia.org/wiki/Even%E2%80%93odd_rule
 211
 212 \param x Horizontal coordinate.
 213 \param y Vertical coordinate.
 214 \param poly The vector of coordinates.
 215 */
 216 bool inside(double x, double y, std::vector<std::tuple<double, double>> &poly);
 217
 218 /*! \brief Return intersection of two line segments.
 219
 220 The output is tuple `std::tuple<bool, double, double>`, where the first
 221 value is true when there is an intersection and false otherwise. The
 222 second and third parameters in the return tuple are coordinates of the
 223 intersection.
 224
 225 \see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
 226
 227 \param x1 First line segment first `x` coordinate.
 228 \param y1 First line segment first `y` coordinate.
 229 \param x2 First line segment second `x` coordinate.
 230 \param y2 First line segment second `y` coordinate.
 231 \param x3 Second line segment first `x` coordinate.
 232 \param y3 Second line segment first `y` coordinate.
 233 \param x4 Second line segment second `x` coordinate.
 234 \param y4 Second line segment second `y` coordinate.
 235 */
 236 std::tuple<bool, double, double> intersect(
 237         double x1, double y1,
 238         double x2, double y2,
 239         double x3, double y3,
 240         double x4, double y4
 241 );
 242
 243 #endif /* BCAR_H */