src/bcar.cc

   1 #include "bcar.h"
   2 #include "pslot.h"
   3
   4 // kinematic constraints
   5 bool BicycleCar::drivable(const BicycleCar &bc) const
   6 {
   7     return this->drivable(bc, bc.h(), bc.h());
   8 }
   9 bool BicycleCar::drivable(const BicycleCar &bc, double b, double e) const
  10 {
  11         // assert bc.h() == (b + e) / 2.0
  12         double a_1 = atan2(bc.y() - this->y(), bc.x() - this->x()) - this->h();
  13         while (a_1 < -M_PI)
  14                 a_1 += 2 * M_PI;
  15         while (a_1 > +M_PI)
  16                 a_1 -= 2 * M_PI;
  17         double h_d = bc.h() - this->h();
  18         while (h_d < -M_PI)
  19                 h_d += 2 * M_PI;
  20         while (h_d > +M_PI)
  21                 h_d -= 2 * M_PI;
  22         double a_2 = 0;
  23         if (h_d == 0 && (a_1 == 0 || a_2 == M_PI || a_2 == -M_PI)) {
  24                 return true;
  25         } else if (0 < a_1 && a_1 <= M_PI/2) { // left front
  26                 BicycleCar z(*this); // zone border
  27                 z.h(e);
  28                 h_d = bc.h() - this->h();
  29                 z.rotate(this->ccl().x(), this->ccl().y(), h_d);
  30                 // assert z.h() == bc.h()
  31                 if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
  32                         return true;
  33                 a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
  34                 while (a_2 < -M_PI)
  35                         a_2 += 2 * M_PI;
  36                 while (a_2 > +M_PI)
  37                         a_2 -= 2 * M_PI;
  38                 if (z.h() >= a_2 && a_2 >= this->h())
  39                         return true;
  40         } else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear
  41                 BicycleCar z(*this); // zone border
  42                 z.h(e);
  43                 h_d = bc.h() - this->h();
  44                 z.rotate(this->ccl().x(), this->ccl().y(), h_d);
  45                 // assert z.h() == bc.h()
  46                 if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
  47                         return true;
  48                 a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
  49                 a_2 -= M_PI;
  50                 while (a_2 < -M_PI)
  51                         a_2 += 2 * M_PI;
  52                 while (a_2 > +M_PI)
  53                         a_2 -= 2 * M_PI;
  54                 if (this->h() >= a_2 && a_2 >= z.h())
  55                         return true;
  56         } else if (0 > a_1 && a_1 >= -M_PI/2) { // right front
  57                 BicycleCar z(*this); // zone border
  58                 z.h(b);
  59                 h_d = bc.h() - this->h();
  60                 z.rotate(this->ccr().x(), this->ccr().y(), h_d);
  61                 // assert z.h() == bc.h()
  62                 if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
  63                         return true;
  64                 a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
  65                 while (a_2 < -M_PI)
  66                         a_2 += 2 * M_PI;
  67                 while (a_2 > +M_PI)
  68                         a_2 -= 2 * M_PI;
  69                 if (this->h() >= a_2 && a_2 >= z.h())
  70                         return true;
  71         } else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear
  72                 BicycleCar z(*this); // zone border
  73                 z.h(b);
  74                 h_d = bc.h() - this->h();
  75                 z.rotate(this->ccr().x(), this->ccr().y(), h_d);
  76                 // assert z.h() == bc.h()
  77                 if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
  78                         return true;
  79                 a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
  80                 a_2 -= M_PI;
  81                 while (a_2 < -M_PI)
  82                         a_2 += 2 * M_PI;
  83                 while (a_2 > +M_PI)
  84                         a_2 -= 2 * M_PI;
  85                 if (z.h() >= a_2 && a_2 >= this->h())
  86                         return true;
  87         } else {
  88                 // Not happenning, as ``-pi <= a <= pi``.
  89         }
  90         return false;
  91 }
  92
  93 double BicycleCar::iradi() const
  94 {
  95         return this->mtr() - this->w() / 2;
  96 }
  97
  98 double BicycleCar::ofradi() const
  99 {
 100         return sqrt(pow(this->mtr() + this->w() / 2, 2) + pow(this->df(), 2));
 101 }
 102
 103 double BicycleCar::orradi() const
 104 {
 105         return sqrt(pow(this->mtr() + this->w() / 2, 2) + pow(this->dr(), 2));
 106 }
 107
 108 double BicycleCar::perfect_parking_slot_len() const
 109 {
 110         // see Simon R. Blackburn *The Geometry of Perfect Parking*
 111         // see https://www.ma.rhul.ac.uk/SRBparking
 112         double r = this->ctc() / 2;
 113         double l = this->wb();
 114         double k = this->df() - this->wb();
 115         double w = this->w();
 116         return
 117                 this->l()
 118                 + sqrt(
 119                         (r*r - l*l)
 120                         + pow(l + k, 2)
 121                         - pow(sqrt(r*r - l*l) - w, 2)
 122                 )
 123                 - l
 124                 - k
 125         ;
 126 }
 127
 128 void BicycleCar::set_max_steer()
 129 {
 130         this->st(atan(this->wb() / this->mtr()));
 131 }
 132
 133 // car frame
 134 double BicycleCar::lfx() const
 135 {
 136         double lfx = this->x();
 137         lfx += (this->w() / 2) * cos(this->h() + M_PI / 2);
 138         lfx += this->df() * cos(this->h());
 139         lfx += this->sd() * cos(this->h());
 140         return lfx;
 141 }
 142
 143 double BicycleCar::lfy() const
 144 {
 145         double lfy = this->y();
 146         lfy += (this->w() / 2) * sin(this->h() + M_PI / 2);
 147         lfy += this->df() * sin(this->h());
 148         lfy += this->sd() * sin(this->h());
 149         return lfy;
 150 }
 151
 152 double BicycleCar::lrx() const
 153 {
 154         double lrx = this->x();
 155         lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
 156         lrx += -this->dr() * cos(this->h());
 157         lrx += -this->sd() * cos(this->h());
 158         return lrx;
 159 }
 160
 161 double BicycleCar::lry() const
 162 {
 163         double lry = this->y();
 164         lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
 165         lry += -this->dr() * sin(this->h());
 166         lry += -this->sd() * sin(this->h());
 167         return lry;
 168 }
 169
 170 double BicycleCar::rrx() const
 171 {
 172         double rrx = this->x();
 173         rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
 174         rrx += -this->dr() * cos(this->h());
 175         rrx += -this->sd() * cos(this->h());
 176         return rrx;
 177 }
 178
 179 double BicycleCar::rry() const
 180 {
 181         double rry = this->y();
 182         rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
 183         rry += -this->dr() * sin(this->h());
 184         rry += -this->sd() * sin(this->h());
 185         return rry;
 186 }
 187
 188 double BicycleCar::rfx() const
 189 {
 190         double rfx = this->x();
 191         rfx += (this->w() / 2) * cos(this->h() - M_PI / 2);
 192         rfx += this->df() * cos(this->h());
 193         rfx += this->sd() * cos(this->h());
 194         return rfx;
 195 }
 196
 197 double BicycleCar::rfy() const
 198 {
 199         double rfy = this->y();
 200         rfy += (this->w() / 2) * sin(this->h() - M_PI / 2);
 201         rfy += this->df() * sin(this->h());
 202         rfy += this->sd() * sin(this->h());
 203         return rfy;
 204 }
 205
 206 double BicycleCar::ralx() const
 207 {
 208         double lrx = this->x();
 209         lrx += (this->w() / 2) * cos(this->h() + M_PI / 2);
 210         return lrx;
 211 }
 212 double BicycleCar::raly() const
 213 {
 214         double lry = this->y();
 215         lry += (this->w() / 2) * sin(this->h() + M_PI / 2);
 216         return lry;
 217 }
 218
 219 double BicycleCar::rarx() const
 220 {
 221         double rrx = this->x();
 222         rrx += (this->w() / 2) * cos(this->h() - M_PI / 2);
 223         return rrx;
 224 }
 225
 226 double BicycleCar::rary() const
 227 {
 228         double rry = this->y();
 229         rry += (this->w() / 2) * sin(this->h() - M_PI / 2);
 230         return rry;
 231 }
 232
 233 BicycleCar BicycleCar::ccl() const
 234 {
 235         BicycleCar bc;
 236         bc.x(this->x() + this->mtr() * cos(this->h() + M_PI / 2));
 237         bc.y(this->y() + this->mtr() * sin(this->h() + M_PI / 2));
 238         bc.h(this->h());
 239         return bc;
 240 }
 241
 242 BicycleCar BicycleCar::ccr() const
 243 {
 244         BicycleCar bc;
 245         bc.x(this->x() + this->mtr() * cos(this->h() - M_PI / 2));
 246         bc.y(this->y() + this->mtr() * sin(this->h() - M_PI / 2));
 247         bc.h(this->h());
 248         return bc;
 249 }
 250
 251 // moving
 252 void BicycleCar::next()
 253 {
 254         this->x(this->x() + this->sp() * cos(this->h()));
 255         this->y(this->y() + this->sp() * sin(this->h()));
 256         this->h(this->h() + this->sp() / this->wb() * tan(this->st()));
 257 }
 258
 259 void BicycleCar::rotate(double cx, double cy, double angl)
 260 {
 261         double px = this->x();
 262         double py = this->y();
 263         px -= cx;
 264         py -= cy;
 265         double nx = px * cos(angl) - py * sin(angl);
 266         double ny = px * sin(angl) + py * cos(angl);
 267         this->h(this->h() + angl);
 268         this->x(nx + cx);
 269         this->y(ny + cy);
 270 }
 271
 272 BicycleCar::BicycleCar()
 273 {
 274         // TODO according to mtr_ FIXME
 275         this->mtr_ = sqrt(
 276                         pow(10.82 / 2, 2)
 277                         - pow(this->wb(), 2)
 278                 )
 279                 - this->w() / 2
 280         ;
 281 }
 282
 283 std::tuple<bool, unsigned int, unsigned int>
 284 collide(
 285         std::vector<std::tuple<double, double>> &p1,
 286         std::vector<std::tuple<double, double>> &p2
 287 )
 288 {
 289         for (unsigned int i = 0; i < p1.size() - 1; i++) {
 290                 for (unsigned int j = 0; j < p2.size() - 1; j++) {
 291                         auto x = intersect(
 292                                 std::get<0>(p1[i]),
 293                                 std::get<1>(p1[i]),
 294                                 std::get<0>(p1[i + 1]),
 295                                 std::get<1>(p1[i + 1]),
 296                                 std::get<0>(p2[j]),
 297                                 std::get<1>(p2[j]),
 298                                 std::get<0>(p2[j + 1]),
 299                                 std::get<1>(p2[j + 1])
 300                         );
 301                         if (std::get<0>(x))
 302                                 return std::make_tuple(true, i, j);
 303                 }
 304         }
 305         return std::make_tuple(false, 0, 0);
 306 }
 307
 308 bool
 309 inside(double x, double y, std::vector<std::tuple<double, double>> &poly)
 310 {
 311         unsigned int i = 0;
 312         unsigned int j = 3;
 313         bool inside = false;
 314         for (i = 0; i < 4; i++) {
 315                 if (
 316                         (std::get<1>(poly[i]) > y) != (std::get<1>(poly[j]) > y)
 317                         && (
 318                                 x < std::get<0>(poly[i])
 319                                 + (std::get<0>(poly[j]) - std::get<0>(poly[i]))
 320                                 * (y - std::get<1>(poly[i]))
 321                                 / (std::get<1>(poly[j]) - std::get<1>(poly[i]))
 322                         )
 323                 )
 324                         inside = !inside;
 325                 j = i;
 326         }
 327         return inside;
 328 }
 329
 330 std::tuple<bool, double, double>
 331 intersect(
 332         double x1, double y1,
 333         double x2, double y2,
 334         double x3, double y3,
 335         double x4, double y4
 336 )
 337 {
 338         double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
 339         if (deno == 0)
 340                 return std::make_tuple(false, 0, 0);
 341         double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
 342         t /= deno;
 343         double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
 344         u *= -1;
 345         u /= deno;
 346         if (t < 0 || t > 1 || u < 0 || u > 1)
 347                 return std::make_tuple(false, 0, 0);
 348         return std::make_tuple(true, x1 + t * (x2 - x1), y1 + t * (y2 - y1));
 349 }
 350
 351 std::tuple<bool, double, double, double, double>
 352 intersect(
 353         double cx, double cy, double r,
 354         double x1, double y1,
 355         double x2, double y2
 356 ) {
 357         x2 -= cx;
 358         x1 -= cx;
 359         y2 -= cy;
 360         y1 -= cy;
 361         if (y1 == y2)
 362             y1 += 0.00001;
 363         double dx = x2 - x1;
 364         double dy = y2 - y1;
 365         double dr = sqrt(dx*dx + dy*dy);
 366         double D = x1*y2 - x2*y1;
 367         if (r*r * dr*dr - D*D < 0)
 368         return std::make_tuple(false, 0, 0, 0, 0);
 369         // intersection coordinates
 370         double ix1 = (D*dy + sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
 371         ix1 += cx;
 372         double ix2 = (D*dy - sgn(dy)*dx*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
 373         ix2 += cx;
 374         double iy1 = (-D*dx + std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
 375         iy1 += cy;
 376         double iy2 = (-D*dx - std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
 377         iy2 += cy;
 378         return std::make_tuple(true, ix1, iy1, ix2, iy2);
 379 }
 380
 381 double
 382 angle_between_three_points(
 383         double x1, double y1,
 384         double x2, double y2,
 385         double x3, double y3
 386 ) {
 387         double d1x = x2 - x1;
 388         double d1y = y2 - y1;
 389         double d2x = x3 - x2;
 390         double d2y = y3 - y2;
 391
 392         double dot = d1x*d2x + d1y*d2y;
 393         double d1 = sqrt(d1x*d1x + d1y*d1y);
 394         double d2 = sqrt(d2x*d2x + d2y*d2y);
 395
 396         double delta = acos(dot / (d1 * d2));
 397         return std::min(delta, M_PI - delta);
 398 }
 399
 400 bool
 401 right_side_of_line(
 402         double x1, double y1,
 403         double x2, double y2,
 404         double x3, double y3
 405 ) {
 406         if (sgn((x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1)) < 0)
 407                 return false;
 408         else
 409                 return true;
 410 }