api/bcar.h

   1 /*! \file */
   2 #ifndef BCAR_H
   3 #define BCAR_H
   4
   5 #include <cmath>
   6 #include <ostream>
   7 #include <tuple>
   8 #include <vector>
   9
  10 /*! \brief Bicycle car basic class.
  11
  12 This class contains some geometrical computations of bicycle car.
  13
  14 \param x Horizontal coordinate of rear axle center.
  15 \param y Vertical coordinate of rear axle center.
  16 \param h Heading of the car in the interval [-pi,+pi] radians.
  17 \param mtr Minimum turning radius.
  18 \param wb Wheelbase.
  19 \param w The width of the car.
  20 \param l The length of the car.
  21 \param he The height of the car.
  22 \param sd The safety distance.
  23 \param df Distance from rear axle center to the front of the car.
  24 \param dr Distance from rear axle center to the back of the car.
  25 \param sp Speed of the car.
  26 \param st Steering of the car.
  27 */
  28 class BicycleCar {
  29 private:
  30         // coordinates
  31         double x_ = 0;
  32         double y_ = 0;
  33         double h_ = 0;
  34         // kinematic constraints
  35         double ctc_ = 10.820; // curb-to-curb
  36         // FIXME is not mtr; curb-to-curb is 10.820
  37         double mtr_ = 10.820;
  38         double wb_ = 2.450;
  39         // dimensions
  40         double w_ = 1.625;
  41         double l_ = 3.760;
  42         double he_ = 1.450;
  43         double sd_ = 0;
  44         double df_ = 3.105;
  45         double dr_ = 0.655;
  46         // moving
  47         double sp_ = 0;
  48         double st_ = 0;
  49 public:
  50         // kinematic constraints
  51         /*! \brief Return `false` if `bc` is not achievable.
  52
  53         When `false` is returned the `bc` may still be drivable,
  54         because only "line segment - circle arc - line segment"
  55         paths are considered in ``drivable`` method.
  56
  57         \param[in] bc The bicycle car to achieve.
  58         */
  59         bool drivable(const BicycleCar &bc) const;
  60         bool drivable(const BicycleCar &bc, double b, double e) const;
  61         /*! \brief Return inner radius.
  62
  63         The inner radius is the distance from minimum turning
  64         radius circle center to the nearest point on the car. In
  65         this case, the nearest points on the car are rear axle
  66         endpoints.
  67         */
  68         double iradi() const;
  69         /*! \brief Return outer front radius.
  70
  71         The outer front radius is the distance from minimum
  72         turning radius circle center to the farthest point on
  73         the front (from the rear axle view) part of the car.
  74         */
  75         double ofradi() const;
  76         /*! \brief Return outer rear radius.
  77
  78         The outer rear radius is the distance from minimum
  79         turning radius circle center to the farthest point on
  80         the rear (from the rear axle view) part of the car.
  81         */
  82         double orradi() const;
  83         /*! \brief Return length of perfect parking slot.
  84
  85         The width of the slot is the same as the width of the
  86         car.
  87         */
  88         double perfect_parking_slot_len() const;
  89         /*! \brief Set maximum steering angle.
  90         */
  91         void set_max_steer();
  92
  93         // car frame
  94         double lfx() const; double lfy() const;
  95         double lrx() const; double lry() const;
  96         double rrx() const; double rry() const;
  97         double rfx() const; double rfy() const;
  98
  99         double ralx() const; double raly() const;
 100         double rarx() const; double rary() const;
 101
 102         /*! \brief Min. turning radius circle center on left.
 103
 104         Important are coordinates `x` and `y`. The heading `h`
 105         is set as the heading of `this->h()`.
 106         */
 107         BicycleCar ccl() const;
 108         /*! \brief Min. turning radius circle center on rigth.
 109
 110         Important are coordinates `x` and `y`. The heading `h`
 111         is set as the heading of `this->h()`.
 112         */
 113         BicycleCar ccr() const;
 114
 115         // moving
 116         /*! \brief Next car position based on `sp` and `st`.
 117
 118         Where `sp` is speed and `st` is steering of the car.
 119         */
 120         void next();
 121         /*! \brief Rotate self around the point.
 122
 123         \param cx Horizontal coordinate of rotation center.
 124         \param cy Vertical coordinate of rotation center.
 125         \param angl Angle of rotation.
 126         */
 127         void rotate(double cx, double cy, double angl);
 128
 129         // getters, setters
 130         double x() const { return this->x_; }
 131         void x(double x) { this->x_ = x; }
 132
 133         double y() const { return this->y_; }
 134         void y(double y) { this->y_ = y; }
 135
 136         double h() const { return this->h_; }
 137         void h(double h)
 138         {
 139                 while (h < -M_PI)
 140                         h += 2 * M_PI;
 141                 while (h > +M_PI)
 142                         h -= 2 * M_PI;
 143                 this->h_ = h;
 144         }
 145
 146         double ctc() const { return this->ctc_; }
 147         void ctc(double ctc) { this->ctc_ = ctc; }
 148
 149         double mtr() const { return this->mtr_; }
 150         void mtr(double mtr) { this->mtr_ = mtr; }
 151
 152         double wb() const { return this->wb_; }
 153         void wb(double wb) { this->wb_ = wb; }
 154
 155         double w() const { return this->w_; }
 156         void w(double w) { this->w_ = w; }
 157
 158         double l() const { return this->l_; }
 159         void l(double l) { this->l_ = l; }
 160
 161         double he() const { return this->he_; }
 162         void he(double he) { this->he_ = he; }
 163
 164         double sd() const { return this->sd_; }
 165         void sd(double sd) { this->sd_ = sd; }
 166
 167         double df() const { return this->df_; }
 168         void df(double df) { this->df_ = df; }
 169
 170         double dr() const { return this->dr_; }
 171         void dr(double dr) { this->dr_ = dr; }
 172
 173         double sp() const { return this->sp_; }
 174         void sp(double sp) { this->sp_ = sp; }
 175
 176         double st() const { return this->st_; }
 177         void st(double st) { this->st_ = st; }
 178
 179         BicycleCar();
 180         friend std::ostream &operator<<(
 181                 std::ostream &out,
 182                 const BicycleCar &bc
 183         )
 184         {
 185                 out << "[" << bc.x();
 186                 out << "," << bc.y();
 187                 out << "," << bc.h();
 188                 out << "]";
 189                 return out;
 190         }
 191 };
 192
 193 /*! \brief Does two polygons collide?
 194
 195 Return the tuple `std::tuple<bool, int, int>`, where the first value is
 196 `true` when there is an intersection of some segments of the polygons
 197 `p1` and `p2` and `false` otherwise. The second and third parameters in
 198 the return tuple are indexes of the first collision, where index starts
 199 at 0.
 200
 201 \param p1 The first polygon to check against collision.
 202 \param p2 The second polygon to check against collision.
 203 */
 204 std::tuple<bool, unsigned int, unsigned int>
 205 collide(
 206         std::vector<std::tuple<double, double>> &p1,
 207         std::vector<std::tuple<double, double>> &p2
 208 );
 209
 210 /*! \brief Is `x, y` coordinate in polygon `poly`?
 211
 212 Return `true` if `x, y` coordinate is inside of polygon `poly`.
 213
 214 \see https://en.wikipedia.org/wiki/Even%E2%80%93odd_rule
 215
 216 \param x Horizontal coordinate.
 217 \param y Vertical coordinate.
 218 \param poly The vector of coordinates.
 219 */
 220 bool
 221 inside(double x, double y, std::vector<std::tuple<double, double>> &poly);
 222
 223 /*! \brief Return intersection of two line segments.
 224
 225 The output is tuple `std::tuple<bool, double, double>`, where the first
 226 value is true when there is an intersection and false otherwise. The
 227 second and third parameters in the return tuple are coordinates of the
 228 intersection.
 229
 230 \see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
 231
 232 \param x1 First line segment first `x` coordinate.
 233 \param y1 First line segment first `y` coordinate.
 234 \param x2 First line segment second `x` coordinate.
 235 \param y2 First line segment second `y` coordinate.
 236 \param x3 Second line segment first `x` coordinate.
 237 \param y3 Second line segment first `y` coordinate.
 238 \param x4 Second line segment second `x` coordinate.
 239 \param y4 Second line segment second `y` coordinate.
 240 */
 241 std::tuple<bool, double, double>
 242 intersect(
 243         double x1, double y1,
 244         double x2, double y2,
 245         double x3, double y3,
 246         double x4, double y4
 247 );
 248
 249 /*! \brief Return intersections of (infinite) line and circle.
 250
 251 The output is tuple `std::tuble<bool, double, double, double, double>`, where
 252 the first value is true when there is an intersection and false otherwise. The
 253 second and third parameters in the return tuple are coordinates of the first
 254 intersection. The fourth and fifth parameters in the return tuple are
 255 coordinates of the second intersection.
 256
 257 \see https://mathworld.wolfram.com/Circle-LineIntersection.html
 258
 259 \param cx Circle center `x` coordinate.
 260 \param cy Circle center `y` coordinate.
 261 \param r Circle radius.
 262 \param x1 Line segment first `x` coordinate.
 263 \param y1 Line segment first `y` coordinate.
 264 \param x2 Line segment second `x` coordinate.
 265 \param y2 Line segment second `y` coordinate.
 266 */
 267 std::tuple<bool, double, double, double, double>
 268 intersect(
 269         double cx, double cy, double r,
 270         double x1, double y1,
 271         double x2, double y2
 272 );
 273
 274 /*! \brief Return the smallest angle between three points.
 275
 276 \see https://math.stackexchange.com/questions/361412/finding-the-angle-between-three-points
 277
 278 \param x1
 279 \param y1
 280 \param x2
 281 \param y2
 282 \param x3
 283 \param y3
 284 */
 285 double
 286 angle_between_three_points(
 287         double x1, double y1,
 288         double x2, double y2,
 289         double x3, double y3
 290 );
 291
 292 /*! \brief Return if point is on the right side of plane.
 293
 294 \param x1 Line first `x` coordinate.
 295 \param y1 Line first `y` coordinate.
 296 \param x2 Line second `x` coordinate.
 297 \param y2 Line second `y` coordinate.
 298 \param x3 Point to decide `x` coordinate.
 299 \param y3 Point to decide `y` coordinate.
 300 */
 301 bool
 302 right_side_of_line(
 303         double x1, double y1,
 304         double x2, double y2,
 305         double x3, double y3
 306 );
 307
 308 #endif /* BCAR_H */