src/psp.cc

   1 #include <cmath>
   2 #include <list>
   3 #include <queue>
   4 #include "psp.h"
   5
   6 bool PSPlanner::collide()
   7 {
   8         std::vector<std::tuple<double, double>> bc;
   9         bc.push_back(std::make_tuple(this->cc().lfx(), this->cc().lfy()));
  10         bc.push_back(std::make_tuple(this->cc().lrx(), this->cc().lry()));
  11         bc.push_back(std::make_tuple(this->cc().rrx(), this->cc().rry()));
  12         bc.push_back(std::make_tuple(this->cc().rfx(), this->cc().rfy()));
  13         bc.push_back(std::make_tuple(this->cc().lfx(), this->cc().lfy()));
  14         std::vector<std::tuple<double, double>> ps;
  15         ps.push_back(std::make_tuple(this->ps().x1(), this->ps().y1()));
  16         ps.push_back(std::make_tuple(this->ps().x2(), this->ps().y2()));
  17         ps.push_back(std::make_tuple(this->ps().x3(), this->ps().y3()));
  18         ps.push_back(std::make_tuple(this->ps().x4(), this->ps().y4()));
  19         return std::get<0>(::collide(bc, ps));
  20 }
  21
  22 bool PSPlanner::forward()
  23 {
  24 #if FORWARD_PARKING > 0
  25         return true;
  26 #else
  27         return false;
  28 #endif
  29 }
  30
  31 void PSPlanner::gc_to_4()
  32 {
  33         double angl_slot = atan2(
  34                 this->ps().y3() - this->ps().y4(),
  35                 this->ps().x3() - this->ps().x4()
  36         );
  37         double angl_delta = M_PI / 2;
  38         if (this->ps().right())
  39                 angl_delta = -M_PI / 2;
  40         double x = this->ps().x4();
  41         double y = this->ps().y4();
  42         x += (this->gc().dr() + 0.01) * cos(angl_slot);
  43         y += (this->gc().dr() + 0.01) * sin(angl_slot);
  44         x += (this->gc().w() / 2 + 0.01) * cos(angl_slot + angl_delta);
  45         y += (this->gc().w() / 2 + 0.01) * sin(angl_slot + angl_delta);
  46         this->gc().x(x);
  47         this->gc().y(y);
  48         this->gc().h(angl_slot);
  49 }
  50
  51 std::tuple<double, double, double, double> circle_line_intersection(
  52         double cx, double cy, double r,
  53         double x1, double y1,
  54         double x2, double y2
  55 )
  56 {
  57         double t = (y2 - y1) / (x2 - x1);
  58         //double a = 1 + pow(t, 2);
  59         //double b = - 2 * cx - 2 * pow(t, 2) * x1 + 2 * t * y1 - 2 * t * cy;
  60         //double c = pow(cx, 2) + pow(t, 2) * pow(x1, 2) - 2 * t * y1 * x1
  61         //        + pow(y1, 2) + 2 * t * cy * x1 - 2 * y1 * cy + pow(cy, 2)
  62         //        - pow(r, 2);
  63         double a = 1 + pow(t, 2);
  64         double b = - 2 * cx + 2 * t * (-t * x1 + y1) - 2 * cy * t;
  65         double c = pow(cx, 2) + pow(cy, 2) - pow(r, 2);
  66         c += pow(-t * x1 + y1, 2);
  67         c += 2 * cy * t * x1 - 2 * cy * y1;
  68         double D = pow(b, 2) - 4 * a * c;
  69         if (D < 0)
  70                 return std::make_tuple(cx, cy, cx, cy);
  71         double res_x1 = (-b + sqrt(D)) / (2 * a);
  72         double res_y1 = t * (res_x1 - x1) + y1;
  73         double res_x2 = (-b - sqrt(D)) / (2 * a);
  74         double res_y2 = t * (res_x2 - x1) + y1;
  75         return std::make_tuple(res_x1, res_y1, res_x2, res_y2);
  76 }
  77
  78 double edist(double x1, double y1, double x2, double y2)
  79 {
  80         return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
  81 }
  82
  83 void PSPlanner::guess_gc()
  84 {
  85         double x = this->ps().x1();
  86         double y = this->ps().y1();
  87         double h = this->ps().heading();
  88         double dts = + M_PI / 2; // direction to slot
  89         if (this->ps().right())
  90                 dts = - M_PI / 2;
  91         if (this->ps().parallel()) {
  92                 x += (this->gc().w() / 2 + 0.01) * cos(h + dts);
  93                 x += (this->gc().dr() + 0.01) * cos(h);
  94                 y += (this->gc().w() / 2 + 0.01) * sin(h + dts);
  95                 y += (this->gc().dr() + 0.01) * sin(h);
  96         } else {
  97 #if FORWARD_PARKING > 0
  98                 // Forward parking
  99                 double entry_width = edist(
 100                         this->ps().x1(), this->ps().y1(),
 101                         this->ps().x4(), this->ps().y4()
 102                 );
 103                 x += entry_width / 2 * cos(h);
 104                 y += entry_width / 2 * sin(h);
 105                 h = atan2(
 106                         this->ps().y2() - this->ps().y1(),
 107                         this->ps().x2() - this->ps().x1()
 108                 );
 109                 while (h < 0) h += 2 * M_PI;
 110                 x += 2 * this->gc().dr() * cos(h);
 111                 y += 2 * this->gc().dr() * sin(h);
 112 #else
 113                 // Backward parking
 114                 double entry_width = edist(
 115                         this->ps().x1(), this->ps().y1(),
 116                         this->ps().x4(), this->ps().y4()
 117                 );
 118                 x += entry_width / 2 * cos(h);
 119                 y += entry_width / 2 * sin(h);
 120                 h = atan2(
 121                         this->ps().y1() - this->ps().y2(),
 122                         this->ps().x1() - this->ps().x2()
 123                 );
 124                 while (h < 0) h += 2 * M_PI;
 125                 x += this->gc().df() * cos(h + M_PI);
 126                 y += this->gc().df() * sin(h + M_PI);
 127 #endif
 128         }
 129         while (h > M_PI)
 130                 h -= 2 * M_PI;
 131         while (h <= -M_PI)
 132                 h += 2 * M_PI;
 133         this->gc().x(x);
 134         this->gc().y(y);
 135         this->gc().h(h);
 136 }
 137
 138 std::vector<BicycleCar> PSPlanner::last_maneuver()
 139 {
 140         std::vector<BicycleCar> lm;
 141         if (this->ps().parallel()) {
 142                 // zig-zag out from the slot
 143                 this->cc() = BicycleCar(this->gc());
 144                 this->cc().sp(0.1);
 145                 lm.push_back(BicycleCar(this->cc()));
 146                 while (!this->left()) {
 147                         while (!this->collide() && !this->left()) {
 148                                 this->cc().next();
 149                                 lm.push_back(BicycleCar(this->cc()));
 150                         }
 151                         if (this->left() && !this->collide()) {
 152                                 break;
 153                         } else {
 154                                 lm.pop_back();
 155                                 this->cc().sp(this->cc().sp() * -1);
 156                                 this->cc().next();
 157                                 this->cc().st(this->cc().st() * -1);
 158                                 this->c_++;
 159                                 lm.push_back(BicycleCar(this->cc()));
 160                         }
 161                 }
 162                 if (this->cc().st() < 0) {
 163                         this->c_++;
 164                         lm.push_back(BicycleCar(this->cc()));
 165                 }
 166         } else {
 167                 // go 0.1 m forward/backward
 168 #if FORWARD_PARKING > 0
 169                 this->cc().sp(-0.1);
 170 #else
 171                 this->cc().sp(0.1);
 172 #endif
 173                 BicycleCar orig_cc(this->cc());
 174                 for (unsigned int i = 0; i < 10; i++) {
 175                         this->cc().next();
 176                         lm.push_back(BicycleCar(this->cc()));
 177                 }
 178                 this->cc() = BicycleCar(orig_cc);
 179         }
 180         return lm;
 181 }
 182
 183 bool PSPlanner::left()
 184 {
 185         double lfx = this->cc().lfx();
 186         double lfy = this->cc().lfy();
 187         double lrx = this->cc().lrx();
 188         double lry = this->cc().lry();
 189         double rrx = this->cc().rrx();
 190         double rry = this->cc().rry();
 191         double rfx = this->cc().rfx();
 192         double rfy = this->cc().rfy();
 193         double lfs = sgn(
 194                 (lfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 195                 - (lfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 196         );
 197         double lrs = sgn(
 198                 (lrx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 199                 - (lry - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 200         );
 201         double rrs = sgn(
 202                 (rrx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 203                 - (rry - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 204         );
 205         double rfs = sgn(
 206                 (rfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
 207                 - (rfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
 208         );
 209         if (this->ps().parallel())
 210                 return lfs == rfs && (lfs != lrs || lfs != rrs);
 211         else if (!this->forward())
 212                 return lfs == rfs && (lfs != lrs || lfs != rrs);
 213         else
 214                 return lrs == rrs && (lrs != lfs || lrs != rfs);
 215 }
 216
 217 bool PSPlanner::parked()
 218 {
 219         std::vector<std::tuple<double, double>> slot;
 220         slot.push_back(std::make_tuple(this->ps().x1(), this->ps().y1()));
 221         slot.push_back(std::make_tuple(this->ps().x2(), this->ps().y2()));
 222         slot.push_back(std::make_tuple(this->ps().x3(), this->ps().y3()));
 223         slot.push_back(std::make_tuple(this->ps().x4(), this->ps().y4()));
 224         return inside(this->gc().lfx(), this->gc().lfy(), slot)
 225                 && inside(this->gc().lrx(), this->gc().lry(), slot)
 226                 && inside(this->gc().rrx(), this->gc().rry(), slot)
 227                 && inside(this->gc().rfx(), this->gc().rfy(), slot);
 228 }
 229
 230 std::vector<BicycleCar> PSPlanner::possible_goals(
 231         unsigned int cnt,
 232         double dist
 233 )
 234 {
 235         std::vector<BicycleCar> pi;
 236         if (this->ps().parallel())
 237                 this->cc().sp(1);
 238         else
 239                 this->cc().sp(-1);
 240         this->cc().sp(this->cc().sp() * dist);
 241         BicycleCar orig_cc(this->cc());
 242         for (unsigned int i = 0; i < cnt; i++) {
 243                 this->cc().next();
 244                 pi.push_back(BicycleCar(this->cc()));
 245         }
 246         this->cc() = BicycleCar(orig_cc);
 247         if (this->ps().parallel()) {
 248                 this->cc().st(0);
 249                 for (unsigned int i = 0; i < cnt; i++) {
 250                         this->cc().next();
 251                         pi.push_back(BicycleCar(this->cc()));
 252                 }
 253                 this->cc() = BicycleCar(orig_cc);
 254         } else {
 255                 if (!this->ps().right()) {
 256                         this->cc().set_max_steer();
 257                         for (unsigned int i = 0; i < cnt; i++) {
 258                                 this->cc().next();
 259                                 pi.push_back(BicycleCar(this->cc()));
 260                         }
 261                 } else {
 262                         this->cc().set_max_steer();
 263                         this->cc().st(this->cc().st() * -1);
 264                         for (unsigned int i = 0; i < cnt; i++) {
 265                                 this->cc().next();
 266                                 pi.push_back(BicycleCar(this->cc()));
 267                         }
 268                 }
 269                 this->cc() = BicycleCar(orig_cc);
 270         }
 271         return pi;
 272 }
 273
 274 void PSPlanner::shrink_to_perfect_len()
 275 {
 276         if (!this->ps().parallel())
 277                 return;
 278         double perfect_len = this->gc().perfect_parking_slot_len();
 279         if (edist(
 280                 this->ps().x1(), this->ps().y1(),
 281                 this->ps().x4(), this->ps().y4()
 282         ) < perfect_len)
 283                 return;
 284         double h = this->ps().heading();
 285         h -= M_PI;
 286         while (h < 0) h += 2 * M_PI;
 287         double ch = perfect_len * cos(h);
 288         double sh = perfect_len * sin(h);
 289         this->ps().border(
 290                 this->ps().x4() + ch, this->ps().y4() + sh,
 291                 this->ps().x3() + ch, this->ps().y3() + sh,
 292                 this->ps().x3(), this->ps().y3(),
 293                 this->ps().x4(), this->ps().y4()
 294         );
 295 }
 296
 297 // find entry
 298 void PSPlanner::fe()
 299 {
 300         this->c_ = 0;
 301         if (this->ps().parallel()) {
 302                 return this->fe_parallel();
 303         } else {
 304                 this->guess_gc();
 305                 this->cc() = BicycleCar(this->gc());
 306                 //this->cc().set_max_steer();
 307                 //if (this->ps().right())
 308                 //        this->cc().st(this->cc().st() * -1);
 309         }
 310 }
 311
 312 double angle_between_closer_point(
 313         double sx, double sy, // common start point
 314         double cx, double cy, // common middle point
 315         double x1, double y1, // first ending point
 316         double x2, double y2 // second ending point
 317 ) {
 318         if (edist(sx, sy, x1, y1) < edist(sx, sy, x2, y2))
 319                 return ::angle_between_three_points(sx, sy, cx, cy, x1, y1);
 320         else
 321                 return ::angle_between_three_points(sx, sy, cx, cy, x2, y2);
 322 }
 323
 324 void PSPlanner::fe_parallel()
 325 {
 326         this->shrink_to_perfect_len();
 327         BicycleCar bco = BicycleCar(this->gc());
 328         this->cc() = BicycleCar();
 329         this->cc().sp(-0.01);
 330         this->cc().set_max_steer();
 331         if (!this->ps().right())
 332                 this->cc().st(this->cc().st() * -1);
 333         this->cc().h(this->ps().heading());
 334         double angl_in_slot = this->ps().heading() - M_PI / 4;
 335         if (!this->ps().right())
 336                 angl_in_slot += M_PI / 2;
 337         this->cc().x(
 338                 this->ps().x4()
 339                 + this->cc().w()/2 * cos(
 340                         this->ps().heading()
 341                         + (this->ps().right() ? + M_PI / 2 : - M_PI / 2)
 342                 )
 343                 + (this->cc().df() + 0.01) * cos(
 344                         this->ps().heading() + M_PI
 345                 )
 346         );
 347         this->cc().y(
 348                 this->ps().y4()
 349                 + this->cc().w()/2 * sin(
 350                         this->ps().heading()
 351                         + (this->ps().right() ? + M_PI / 2 : - M_PI / 2)
 352                 )
 353                 + (this->cc().df() + 0.01) * sin(
 354                         this->ps().heading() + M_PI
 355                 )
 356         );
 357
 358         std::queue<BicycleCar, std::list<BicycleCar>> q;
 359         while (!this->collide()) {
 360                 q.push(this->cc());
 361                 this->cc().rotate(
 362                         this->ps().x4(),
 363                         this->ps().y4() - 0.01,
 364                         ((this->ps().right()) ? 0.001 : -0.001)
 365                 );
 366         }
 367         // BFS - find entry current car `cc` and corresponding goal car `gc`
 368         unsigned int iter_cntr = 0;
 369         while (!q.empty() && iter_cntr < 30) {
 370                 this->cc() = BicycleCar(q.front());
 371                 q.pop();
 372                 if (this->ps().right() && this->cc().sp() < 0) {
 373                         double cclx = this->cc().ccl().x();
 374                         double ccly = this->cc().ccl().y();
 375                         double ccl_lr = edist(
 376                                 cclx, ccly,
 377                                 this->cc().lrx(), this->cc().lry()
 378                         );
 379                         double ccl_rr = edist(
 380                                 cclx, ccly,
 381                                 this->cc().rrx(), this->cc().rry()
 382                         );
 383                         double ccl_p1 = edist(
 384                                 cclx, ccly,
 385                                 this->ps().x1(), this->ps().y1()
 386                         );
 387                         if (ccl_rr < ccl_p1) {
 388                                 // pass parking slot
 389                                 continue;
 390                         } else if (ccl_rr >= ccl_p1 && ccl_lr < ccl_p1) {
 391                                 // partially out of parking slot
 392                                 auto cli1 = ::intersect(
 393                                         cclx, ccly, ccl_p1,
 394                                         this->cc().lrx(), this->cc().lry(),
 395                                         this->cc().rrx(), this->cc().rry()
 396                                 );
 397                                 double a1 = ::angle_between_closer_point(
 398                                         this->ps().x1(), this->ps().y1(),
 399                                         cclx, ccly,
 400                                         std::get<1>(cli1), std::get<2>(cli1),
 401                                         std::get<3>(cli1), std::get<4>(cli1)
 402                                 );
 403                                 auto cli2 = ::intersect(
 404                                         cclx, ccly, ccl_rr,
 405                                         this->ps().x2(), this->ps().y2(),
 406                                         this->ps().x3(), this->ps().y3()
 407                                 );
 408                                 double a2 = angle_between_closer_point(
 409                                         this->cc().rrx(), this->cc().rry(),
 410                                         cclx, ccly,
 411                                         std::get<1>(cli2), std::get<2>(cli2),
 412                                         std::get<3>(cli2), std::get<4>(cli2)
 413                                 );
 414                                 if (std::get<0>(cli1) && (
 415                                         !std::get<0>(cli2)
 416                                         || a1 < a2
 417                                 )) {
 418                                         this->cc().rotate(cclx, ccly, -a1);
 419                                         if (::right_side_of_line(
 420                                                 this->cc().x(), this->cc().y(),
 421
 422                                                 this->cc().x()
 423                                                 + cos(this->ps().heading()),
 424                                                 this->cc().y()
 425                                                 + sin(this->ps().heading()),
 426
 427                                                 this->cc().x()
 428                                                 + cos(this->cc().h()),
 429                                                 this->cc().y()
 430                                                 + sin(this->cc().h())
 431                                         )) {
 432                                                 continue;
 433                                         }
 434                                 } else if (std::get<0>(cli2) && (
 435                                         !std::get<0>(cli1)
 436                                         || a2 < a1
 437                                 )) {
 438                                         this->cc().rotate(cclx, ccly, -a2);
 439                                 } else {
 440                                         continue;
 441                                 }
 442                         } else if (ccl_lr >= ccl_p1) {
 443                                 // in parking slot
 444                                 auto cli1 = ::intersect(
 445                                         cclx, ccly, ccl_lr,
 446                                         this->ps().x1(), this->ps().y1(),
 447                                         this->ps().x2(), this->ps().y2()
 448                                 );
 449                                 double a1 = angle_between_closer_point(
 450                                         this->cc().lrx(), this->cc().lry(),
 451                                         cclx, ccly,
 452                                         std::get<1>(cli1), std::get<2>(cli1),
 453                                         std::get<3>(cli1), std::get<4>(cli1)
 454                                 );
 455                                 auto cli2 = ::intersect(
 456                                         cclx, ccly, ccl_rr,
 457                                         this->ps().x2(), this->ps().y2(),
 458                                         this->ps().x3(), this->ps().y3()
 459                                 );
 460                                 double a2 = angle_between_closer_point(
 461                                         this->cc().rrx(), this->cc().rry(),
 462                                         cclx, ccly,
 463                                         std::get<1>(cli2), std::get<2>(cli2),
 464                                         std::get<3>(cli2), std::get<4>(cli2)
 465                                 );
 466                                 if (std::get<0>(cli1) && (
 467                                         !std::get<0>(cli2)
 468                                         || a1 < a2
 469                                 )) {
 470                                         this->cc().rotate(cclx, ccly, -a1);
 471                                         if (::right_side_of_line(
 472                                                 this->cc().x(), this->cc().y(),
 473
 474                                                 this->cc().x()
 475                                                 + cos(this->ps().heading()),
 476                                                 this->cc().y()
 477                                                 + sin(this->ps().heading()),
 478
 479                                                 this->cc().x()
 480                                                 + cos(this->cc().h()),
 481                                                 this->cc().y()
 482                                                 + sin(this->cc().h())
 483                                         )) {
 484                                                 continue;
 485                                         }
 486                                 } else if (std::get<0>(cli2) && (
 487                                         !std::get<0>(cli1)
 488                                         || a2 < a1
 489                                 )) {
 490                                         this->cc().rotate(cclx, ccly, -a2);
 491                                 } else {
 492                                         continue;
 493                                 }
 494                         }
 495                 } else if (this->ps().right() && this->cc().sp() > 0) {
 496                         double ccrx = this->cc().ccr().x();
 497                         double ccry = this->cc().ccr().y();
 498                         double ccr_lf = edist(
 499                                 ccrx, ccry,
 500                                 this->cc().lfx(), this->cc().lfy()
 501                         );
 502                         double ccr_rf = edist(
 503                                 ccrx, ccry,
 504                                 this->cc().rfx(), this->cc().rfy()
 505                         );
 506                         {
 507                                 double af = std::abs(
 508                                         this->ps().heading()
 509                                         - this->cc().h()
 510                                 );
 511                                 auto tmp_cc = BicycleCar(this->cc());
 512                                 this->cc().rotate(ccrx, ccry, -af);
 513                                 this->gc() = BicycleCar(this->cc());
 514                                 if (
 515                                         !this->collide()
 516                                         && this->parked()
 517                                 ) {
 518                                         this->cc().sp(this->cc().sp() * -1);
 519                                         this->gc() = BicycleCar(this->cc());
 520                                         goto successfinish;
 521                                 } else {
 522                                         this->cc() = BicycleCar(tmp_cc);
 523                                 }
 524                         }
 525                         auto cli1 = ::intersect(
 526                                 ccrx, ccry, ccr_rf,
 527                                 this->ps().x3(), this->ps().y3(),
 528                                 this->ps().x4(), this->ps().y4()
 529                         );
 530                         double a1 = angle_between_closer_point(
 531                                 this->cc().rfx(), this->cc().rfy(),
 532                                 ccrx, ccry,
 533                                 std::get<1>(cli1), std::get<2>(cli1),
 534                                 std::get<3>(cli1), std::get<4>(cli1)
 535                         );
 536                         auto cli2 = ::intersect(
 537                                 ccrx, ccry, ccr_rf,
 538                                 this->ps().x2(), this->ps().y2(),
 539                                 this->ps().x3(), this->ps().y3()
 540                         );
 541                         double a2 = angle_between_closer_point(
 542                                 this->cc().rfx(), this->cc().rfy(),
 543                                 ccrx, ccry,
 544                                 std::get<1>(cli2), std::get<2>(cli2),
 545                                 std::get<3>(cli2), std::get<4>(cli2)
 546                         );
 547                         auto cli3 = ::intersect(
 548                                 ccrx, ccry, ccr_lf,
 549                                 this->ps().x3(), this->ps().y3(),
 550                                 this->ps().x4(), this->ps().y4()
 551                         );
 552                         double a3 = angle_between_closer_point(
 553                                 this->cc().lfx(), this->cc().lfy(),
 554                                 ccrx, ccry,
 555                                 std::get<1>(cli3), std::get<2>(cli3),
 556                                 std::get<3>(cli3), std::get<4>(cli3)
 557                         );
 558                         if (std::get<0>(cli1) && (
 559                             (!std::get<0>(cli2) && !std::get<0>(cli3))
 560                             || (a1 < a2 && !std::get<0>(cli3))
 561                             || (a1 < a3 && !std::get<0>(cli2))
 562                             || (a1 < a2 && a1 < a3)
 563                         )) {
 564                             this->cc().rotate(ccrx, ccry, -a1);
 565                         } else if (std::get<0>(cli2) && (
 566                             (!std::get<0>(cli1) && !std::get<0>(cli3))
 567                             || (a2 < a1 && !std::get<0>(cli3))
 568                             || (a2 < a3 && !std::get<0>(cli1))
 569                             || (a2 < a1 && a2 < a3)
 570                         )) {
 571                             this->cc().rotate(ccrx, ccry, -a2);
 572                         } else if (std::get<0>(cli3) && (
 573                             (!std::get<0>(cli1) && !std::get<0>(cli2))
 574                             || (a3 < a1 && !std::get<0>(cli2))
 575                             || (a3 < a2 && !std::get<0>(cli1))
 576                             || (a3 < a1 && a3 < a2)
 577                         )) {
 578                             this->cc().rotate(ccrx, ccry, -a3);
 579                         } else {
 580                             continue;
 581                         }
 582                 } else if (!this->ps().right() && this->cc().sp() < 0) {
 583                         double ccrx = this->cc().ccr().x();
 584                         double ccry = this->cc().ccr().y();
 585                         double ccr_rr = edist(
 586                                 ccrx, ccry,
 587                                 this->cc().rrx(), this->cc().rry()
 588                         );
 589                         double ccr_lr = edist(
 590                                 ccrx, ccry,
 591                                 this->cc().lrx(), this->cc().lry()
 592                         );
 593                         double ccr_p1 = edist(
 594                                 ccrx, ccry,
 595                                 this->ps().x1(), this->ps().y1()
 596                         );
 597                         if (ccr_lr < ccr_p1) {
 598                                 // pass parking slot
 599                                 continue;
 600                         } else if (ccr_lr >= ccr_p1 && ccr_rr < ccr_p1) {
 601                                 // partially out of parking slot
 602                                 auto cli1 = ::intersect(
 603                                         ccrx, ccry, ccr_p1,
 604                                         this->cc().lrx(), this->cc().lry(),
 605                                         this->cc().rrx(), this->cc().rry()
 606                                 );
 607                                 double a1 = ::angle_between_closer_point(
 608                                         this->ps().x1(), this->ps().y1(),
 609                                         ccrx, ccry,
 610                                         std::get<1>(cli1), std::get<2>(cli1),
 611                                         std::get<3>(cli1), std::get<4>(cli1)
 612                                 );
 613                                 auto cli2 = ::intersect(
 614                                         ccrx, ccry, ccr_lr,
 615                                         this->ps().x2(), this->ps().y2(),
 616                                         this->ps().x3(), this->ps().y3()
 617                                 );
 618                                 double a2 = angle_between_closer_point(
 619                                         this->cc().lrx(), this->cc().lry(),
 620                                         ccrx, ccry,
 621                                         std::get<1>(cli2), std::get<2>(cli2),
 622                                         std::get<3>(cli2), std::get<4>(cli2)
 623                                 );
 624                                 if (std::get<0>(cli1) && (
 625                                         !std::get<0>(cli2)
 626                                         || a1 < a2
 627                                 )) {
 628                                         this->cc().rotate(ccrx, ccry, a1);
 629                                         if (!::right_side_of_line(
 630                                                 this->cc().x(), this->cc().y(),
 631
 632                                                 this->cc().x()
 633                                                 + cos(this->ps().heading()),
 634                                                 this->cc().y()
 635                                                 + sin(this->ps().heading()),
 636
 637                                                 this->cc().x()
 638                                                 + cos(this->cc().h()),
 639                                                 this->cc().y()
 640                                                 + sin(this->cc().h())
 641                                         )) {
 642                                                 continue;
 643                                         }
 644                                 } else if (std::get<0>(cli2) && (
 645                                         !std::get<0>(cli1)
 646                                         || a2 < a1
 647                                 )) {
 648                                         this->cc().rotate(ccrx, ccry, a2);
 649                                 } else {
 650                                         continue;
 651                                 }
 652                         } else if (ccr_rr >= ccr_p1) {
 653                                 // in parking slot
 654                                 auto cli1 = ::intersect(
 655                                         ccrx, ccry, ccr_rr,
 656                                         this->ps().x1(), this->ps().y1(),
 657                                         this->ps().x2(), this->ps().y2()
 658                                 );
 659                                 double a1 = angle_between_closer_point(
 660                                         this->cc().rrx(), this->cc().rry(),
 661                                         ccrx, ccry,
 662                                         std::get<1>(cli1), std::get<2>(cli1),
 663                                         std::get<3>(cli1), std::get<4>(cli1)
 664                                 );
 665                                 auto cli2 = ::intersect(
 666                                         ccrx, ccry, ccr_lr,
 667                                         this->ps().x2(), this->ps().y2(),
 668                                         this->ps().x3(), this->ps().y3()
 669                                 );
 670                                 double a2 = angle_between_closer_point(
 671                                         this->cc().lrx(), this->cc().lry(),
 672                                         ccrx, ccry,
 673                                         std::get<1>(cli2), std::get<2>(cli2),
 674                                         std::get<3>(cli2), std::get<4>(cli2)
 675                                 );
 676                                 if (std::get<0>(cli1) && (
 677                                         !std::get<0>(cli2)
 678                                         || a1 < a2
 679                                 )) {
 680                                         this->cc().rotate(ccrx, ccry, a1);
 681                                         if (!::right_side_of_line(
 682                                                 this->cc().x(), this->cc().y(),
 683
 684                                                 this->cc().x()
 685                                                 + cos(this->ps().heading()),
 686                                                 this->cc().y()
 687                                                 + sin(this->ps().heading()),
 688
 689                                                 this->cc().x()
 690                                                 + cos(this->cc().h()),
 691                                                 this->cc().y()
 692                                                 + sin(this->cc().h())
 693                                         )) {
 694                                                 continue;
 695                                         }
 696                                 } else if (std::get<0>(cli2) && (
 697                                         !std::get<0>(cli1)
 698                                         || a2 < a1
 699                                 )) {
 700                                         this->cc().rotate(ccrx, ccry, a2);
 701                                 } else {
 702                                         continue;
 703                                 }
 704                         }
 705                 } else if (!this->ps().right() && this->cc().sp() > 0) {
 706                         double cclx = this->cc().ccl().x();
 707                         double ccly = this->cc().ccl().y();
 708                         double ccl_rf = edist(
 709                                 cclx, ccly,
 710                                 this->cc().rfx(), this->cc().rfy()
 711                         );
 712                         double ccl_lf = edist(
 713                                 cclx, ccly,
 714                                 this->cc().lfx(), this->cc().lfy()
 715                         );
 716                         {
 717                                 double af = std::abs(
 718                                         this->ps().heading()
 719                                         - this->cc().h()
 720                                 );
 721                                 auto tmp_cc = BicycleCar(this->cc());
 722                                 this->cc().rotate(cclx, ccly, af);
 723                                 this->gc() = BicycleCar(this->cc());
 724                                 if (
 725                                         !this->collide()
 726                                         && this->parked()
 727                                 ) {
 728                                         this->cc().sp(this->cc().sp() * -1);
 729                                         this->gc() = BicycleCar(this->cc());
 730                                         goto successfinish;
 731                                 } else {
 732                                         this->cc() = BicycleCar(tmp_cc);
 733                                 }
 734                         }
 735                         auto cli1 = ::intersect(
 736                                 cclx, ccly, ccl_lf,
 737                                 this->ps().x3(), this->ps().y3(),
 738                                 this->ps().x4(), this->ps().y4()
 739                         );
 740                         double a1 = angle_between_closer_point(
 741                                 this->cc().lfx(), this->cc().lfy(),
 742                                 cclx, ccly,
 743                                 std::get<1>(cli1), std::get<2>(cli1),
 744                                 std::get<3>(cli1), std::get<4>(cli1)
 745                         );
 746                         auto cli2 = ::intersect(
 747                                 cclx, ccly, ccl_lf,
 748                                 this->ps().x2(), this->ps().y2(),
 749                                 this->ps().x3(), this->ps().y3()
 750                         );
 751                         double a2 = angle_between_closer_point(
 752                                 this->cc().lfx(), this->cc().lfy(),
 753                                 cclx, ccly,
 754                                 std::get<1>(cli2), std::get<2>(cli2),
 755                                 std::get<3>(cli2), std::get<4>(cli2)
 756                         );
 757                         auto cli3 = ::intersect(
 758                                 cclx, ccly, ccl_rf,
 759                                 this->ps().x3(), this->ps().y3(),
 760                                 this->ps().x4(), this->ps().y4()
 761                         );
 762                         double a3 = angle_between_closer_point(
 763                                 this->cc().rfx(), this->cc().rfy(),
 764                                 cclx, ccly,
 765                                 std::get<1>(cli3), std::get<2>(cli3),
 766                                 std::get<3>(cli3), std::get<4>(cli3)
 767                         );
 768                         if (std::get<0>(cli1) && (
 769                             (!std::get<0>(cli2) && !std::get<0>(cli3))
 770                             || (a1 < a2 && !std::get<0>(cli3))
 771                             || (a1 < a3 && !std::get<0>(cli2))
 772                             || (a1 < a2 && a1 < a3)
 773                         )) {
 774                             this->cc().rotate(cclx, ccly, a1);
 775                         } else if (std::get<0>(cli2) && (
 776                             (!std::get<0>(cli1) && !std::get<0>(cli3))
 777                             || (a2 < a1 && !std::get<0>(cli3))
 778                             || (a2 < a3 && !std::get<0>(cli1))
 779                             || (a2 < a1 && a2 < a3)
 780                         )) {
 781                             this->cc().rotate(cclx, ccly, a2);
 782                         } else if (std::get<0>(cli3) && (
 783                             (!std::get<0>(cli1) && !std::get<0>(cli2))
 784                             || (a3 < a1 && !std::get<0>(cli2))
 785                             || (a3 < a2 && !std::get<0>(cli1))
 786                             || (a3 < a1 && a3 < a2)
 787                         )) {
 788                             this->cc().rotate(cclx, ccly, a3);
 789                         } else {
 790                             continue;
 791                         }
 792                 } else {
 793                         // TODO left parking slot (both forward, backward)
 794                 }
 795                 this->cc().sp(this->cc().sp() * -1);
 796                 this->cc().next();
 797                 this->gc() = BicycleCar(this->cc());
 798                 if (this->parked())
 799                         goto successfinish;
 800                 this->cc().st(this->cc().st() * -1);
 801                 q.push(BicycleCar(this->cc()));
 802                 if (sgn(this->cc().st()) == sgn(q.front().st()))
 803                         iter_cntr++;
 804         }
 805         // fallback to fer
 806         this->gc() = BicycleCar(bco);
 807 successfinish:
 808         return this->fer_parallel();
 809 }
 810
 811 void PSPlanner::fe_perpendicular()
 812 {
 813         // TODO Try multiple angles when going from parking slot.
 814         //
 815         //      Do not use just the maximum steer angle. Test angles
 816         //      until the whole current car `cc` is out of the parking
 817         //      slot `ps`.
 818         //
 819         //      Another approach could be testing angles from the
 820         //      beginning of the escape parkig slot maneuver.
 821         if (this->forward())
 822                 this->cc().sp(-0.01);
 823         else
 824                 this->cc().sp(0.01);
 825         while (!this->left())
 826                 this->cc().next();
 827         return;
 828 }
 829
 830 void PSPlanner::fer()
 831 {
 832         this->c_ = 0;
 833         if (this->ps().parallel()) {
 834                 this->guess_gc();
 835                 this->cc() = BicycleCar(this->gc());
 836                 this->cc().set_max_steer();
 837                 if (!this->ps().right())
 838                         this->cc().st(this->cc().st() * -1);
 839                 this->cc().sp(0.01);
 840                 return this->fer_parallel();
 841         } else {
 842                 return this->fer_perpendicular();
 843         }
 844 }
 845
 846 void PSPlanner::fer_parallel()
 847 {
 848         this->cusps_.clear();
 849         while (!this->left()) {
 850                 while (!this->collide() && !this->left())
 851                         this->cc().next();
 852                 if (this->left() && !this->collide()) {
 853                         break;
 854                 } else {
 855                         this->cc().sp(this->cc().sp() * -1);
 856                         this->cc().next();
 857                         this->cc().st(this->cc().st() * -1);
 858                         this->c_++;
 859                         this->cusps_.push_back(this->cc());
 860                 }
 861         }
 862         if (this->cc().st() < 0) {
 863                 this->c_++;
 864                 this->cusps_.push_back(this->cc());
 865         }
 866 }
 867
 868 void PSPlanner::fer_perpendicular()
 869 {
 870         bool delta_use[] = {true, true, true};
 871         double cc_h = this->cc().h();
 872         double x;
 873         double y;
 874         // check inner radius
 875         if (this->forward()) {
 876                 x = this->ps().x1();
 877                 y = this->ps().y1();
 878         } else {
 879                 x = this->ps().x4();
 880                 y = this->ps().y4();
 881         }
 882         double x1;
 883         double y1;
 884         if (this->ps().right()) {
 885                 x1 = this->cc().ccr().x();
 886                 y1 = this->cc().ccr().y();
 887         } else {
 888                 x1 = this->cc().ccl().x();
 889                 y1 = this->cc().ccl().y();
 890         }
 891         double IR = this->cc().iradi();
 892         double a = 1;
 893         double b;
 894         if (this->forward())
 895                 b = (x - x1) * 2 * cos(cc_h) + (y - y1) * 2 * sin(cc_h);
 896         else
 897                 b = (x1 - x) * 2 * cos(cc_h) + (y1 - y) * 2 * sin(cc_h);
 898         double c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
 899         double D = pow(b, 2) - 4 * a * c;
 900         double delta;
 901         delta = -b - sqrt(D);
 902         delta /= 2 * a;
 903         double delta_1 = delta;
 904         if (D < 0)
 905                 delta_use[0] = false;
 906         // check outer radius
 907         if (this->forward()) {
 908                 x = this->ps().x4();
 909                 y = this->ps().y4();
 910         } else {
 911                 x = this->ps().x1();
 912                 y = this->ps().y1();
 913         }
 914         IR = this->cc().ofradi();
 915         a = 1;
 916         if (this->forward())
 917                 b = (x - x1) * 2 * cos(cc_h) + (y - y1) * 2 * sin(cc_h);
 918         else
 919                 b = (x1 - x) * 2 * cos(cc_h) + (y1 - y) * 2 * sin(cc_h);
 920         c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
 921         D = pow(b, 2) - 4 * a * c;
 922         if (this->forward()) {
 923                 delta = -b + sqrt(D);
 924                 delta /= 2 * a;
 925         }
 926         double delta_2 = delta;
 927         if (D < 0)
 928                 delta_use[1] = false;
 929         delta = -b - sqrt(D);
 930         delta /= 2 * a;
 931         double delta_3 = delta;
 932         if (D < 0)
 933                 delta_use[2] = false;
 934         if (delta_use[0] && delta_use[1] && delta_use[2])
 935                 delta = std::max(delta_1, std::max(delta_2, delta_3));
 936         else if (delta_use[0] && delta_use[1])
 937                 delta = std::max(delta_1, delta_2);
 938         else if (delta_use[0] && delta_use[2])
 939                 delta = std::max(delta_1, delta_3);
 940         else if (delta_use[1] && delta_use[2])
 941                 delta = std::max(delta_2, delta_3);
 942         else if (delta_use[0])
 943                 delta = delta_1;
 944         else if (delta_use[1])
 945                 delta = delta_2;
 946         else if (delta_use[2])
 947                 delta = delta_3;
 948         else
 949                 return;
 950         // current car `cc` can get out of slot with max steer
 951         this->cc().x(this->cc().x() + delta * cos(cc_h));
 952         this->cc().y(this->cc().y() + delta * sin(cc_h));
 953         this->cc().h(cc_h);
 954         // get current car `cc` out of slot
 955         if (this->forward())
 956                 this->cc().sp(-0.01);
 957         else
 958                 this->cc().sp(0.01);
 959         this->cc().set_max_steer();
 960         if (this->ps().right())
 961                 this->cc().st(this->cc().st() * -1);
 962         while (!this->left()) {
 963                 while (!this->collide() && !this->left())
 964                         this->cc().next();
 965                 if (this->left() && !this->collide()) {
 966                         break;
 967                 } else {
 968                         this->cc().sp(this->cc().sp() * -1);
 969                         this->cc().next();
 970                         this->cc().st(this->cc().st() * -1);
 971                 }
 972         }
 973 }
 974
 975 PSPlanner::PSPlanner()
 976 {
 977 }