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