decision_control/slotplanner.cc

   1 /*
   2 This file is part of I am car.
   3
   4 I am car is free software: you can redistribute it and/or modify
   5 it under the terms of the GNU General Public License as published by
   6 the Free Software Foundation, either version 3 of the License, or
   7 (at your option) any later version.
   8
   9 I am car is distributed in the hope that it will be useful,
  10 but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 GNU General Public License for more details.
  13
  14 You should have received a copy of the GNU General Public License
  15 along with I am car. If not, see <http://www.gnu.org/licenses/>.
  16 */
  17
  18 #include <algorithm>
  19 #include <cmath>
  20 #include <list>
  21 #include <queue>
  22 #include "slotplanner.h"
  23
  24 ParallelSlot::ParallelSlot()
  25 {}
  26
  27 // getter
  28 std::vector<RRTNode *> &ParallelSlot::goals()
  29 {
  30         return this->goals_;
  31 }
  32
  33 RRTNode *ParallelSlot::getMidd()
  34 {
  35         if (this->cusp().size() > 0)
  36                 return this->cusp().front().front();
  37         else
  38                 return nullptr;
  39 }
  40
  41 std::vector<std::vector<RRTNode *>> &ParallelSlot::cusp()
  42 {
  43         return this->cusp_;
  44 }
  45
  46 float ParallelSlot::DH() const
  47 {
  48         return this->DH_;
  49 }
  50
  51 PolygonObstacle &ParallelSlot::slot()
  52 {
  53         return this->slot_;
  54 }
  55
  56 float ParallelSlot::slotHeading()
  57 {
  58         return this->slotHeading_;
  59 }
  60
  61 SlotSide ParallelSlot::slotSide()
  62 {
  63         return this->slotSide_;
  64 }
  65
  66 SlotType ParallelSlot::slotType()
  67 {
  68         return this->slotType_;
  69 }
  70
  71 float ParallelSlot::poseHeading()
  72 {
  73         return this->poseHeading_;
  74 }
  75
  76 // setter
  77 void ParallelSlot::DH(float dh)
  78 {
  79         this->DH_ = dh;
  80 }
  81
  82 void ParallelSlot::setAll()
  83 {
  84         // slot heading
  85         float y0 = this->slot().bnodes()[0]->y();
  86         float x0 = this->slot().bnodes()[0]->x();
  87         float y3 = this->slot().bnodes()[3]->y();
  88         float x3 = this->slot().bnodes()[3]->x();
  89         float dy = y3 - y0;
  90         float dx = x3 - x0;
  91         this->slotHeading_ = atan2(dy, dx);
  92         // pose heading
  93         float y1 = this->slot().bnodes()[1]->y();
  94         float x1 = this->slot().bnodes()[1]->x();
  95         dy = y0 - y1;
  96         dx = x0 - x1;
  97         this->poseHeading_ = atan2(dy, dx);
  98         // slot side
  99         if (sgn((x1 - x0) * (y3 - y0) - (y1 - y0) * (x3 - x0)) < 0)
 100                 this->slotSide_ = LEFT;
 101         else
 102                 this->slotSide_ = RIGHT;
 103         // slot type
 104         float d1 = EDIST(
 105                 this->slot().bnodes()[0],
 106                 this->slot().bnodes()[1]
 107         );
 108         float d2 = EDIST(
 109                 this->slot().bnodes()[1],
 110                 this->slot().bnodes()[2]
 111         );
 112         if (d1 > d2)
 113                 this->slotType_ = PERPENDICULAR;
 114         else
 115                 this->slotType_ = PARALLEL;
 116 }
 117
 118 // other
 119 void ParallelSlot::fip(
 120         std::vector<CircleObstacle>& co,
 121         std::vector<SegmentObstacle>& so
 122 )
 123 {
 124         this->setAll();
 125         if (this->slotType() == PERPENDICULAR) {
 126                 std::vector<RRTNode *> tmpc;
 127                 BicycleCar *tmpf = this->getFP();
 128                 BicycleCar *tmpb = this->getISPP(tmpf);
 129                 RRTNode *cc;
 130                 if (this->slotSide() == LEFT)
 131                         cc = tmpb->ccl();
 132                 else
 133                         cc = tmpb->ccr();
 134                 if (this->slotSide() == LEFT)
 135                         this->DH(1 * 0.5 / tmpb->out_radi());
 136                 else
 137                         this->DH(-1 * 0.5 / tmpb->out_radi());
 138                 BicycleCar *p;
 139                 int i = 1;
 140                 p = tmpb->move(cc, i * this->DH());
 141                 while (
 142                         !this->slot().collide(p->frame())
 143                         && ((
 144                                 this->slotSide() == LEFT
 145                                 && p->h() < this->slotHeading()
 146                         ) || (
 147                                 this->slotSide() == RIGHT
 148                                 && p->h() > this->slotHeading()
 149                         ))
 150                 ) {
 151                         bool end = false;
 152                         std::vector<RRTEdge *> eds = p->frame();
 153                         for (auto o: co)
 154                                 if (o.collide(eds))
 155                                         end = true;
 156                         for (auto o: so)
 157                                 if (o.collide(eds))
 158                                         end = true;
 159                         for (auto e: eds)
 160                                 delete e;
 161                         if (end)
 162                                 break;
 163                         this->goals_.push_back(p);
 164                         tmpc.push_back(p);
 165                         i += 1;
 166                         p = tmpb->move(cc, i * this->DH());
 167                 }
 168                 if (tmpc.size() > 0)
 169                         this->cusp().push_back(tmpc);
 170                 return;
 171         }
 172         // see https://courses.cs.washington.edu/courses/cse326/03su/homework/hw3/bfs.html
 173         // RRTNode.s() works as iteration level
 174         std::queue<BicycleCar *, std::list<BicycleCar *>> q;
 175         std::queue<BicycleCar *, std::list<BicycleCar *>> empty;
 176         int di = -1;
 177         if (this->slotSide() == LEFT)
 178                 di = 1;
 179         BicycleCar *CC = this->getEPC();
 180         BicycleCar *B = this->getEP();
 181         this->DH(di * 0.01 / CC->out_radi());
 182         BicycleCar *c;
 183         int i = 0;
 184         c = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 185         while (!this->slot().collide(c->frame())) {
 186                 bool end = false;
 187                 std::vector<RRTEdge *> eds = c->frame();
 188                 for (auto o: co)
 189                         if (o.collide(eds))
 190                                 end = true;
 191                 for (auto o: so)
 192                         if (o.collide(eds))
 193                                 end = true;
 194                 for (auto e: eds)
 195                         delete e;
 196                 if (!end)
 197                         q.push(c);
 198                 c = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 199                 i += 1;
 200         }
 201         delete c; // not in q and collide
 202         while (!q.empty()) {
 203                 c = q.front();
 204                 q.pop();
 205                 if (this->isInside(c)) {
 206                         goto createcuspandfinish;
 207                 } else if (c->s() < 9) {
 208                         BicycleCar *cc = this->flnc(c, co, so);
 209                         cc->s(c->s() + 1);
 210                         cc->bcparent(c);
 211                         q.push(cc);
 212                 } else {
 213                         delete c; // not in q and collide
 214                 }
 215         }
 216         std::swap(q, empty);
 217         return;
 218 createcuspandfinish:
 219         std::vector<RRTNode *> cusp;
 220         while (c) {
 221                 cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
 222                 c = c->bcparent();
 223         }
 224         std::reverse(cusp.begin(), cusp.end());
 225         this->cusp().push_back(cusp);
 226         std::swap(q, empty);
 227 }
 228
 229 void ParallelSlot::fipf(
 230         std::vector<CircleObstacle>& co,
 231         std::vector<SegmentObstacle>& so
 232 )
 233 {
 234         this->setAll();
 235         std::vector<RRTNode *> tmpc;
 236         BicycleCar *tmpf = this->getFPf();
 237         BicycleCar *tmpb = this->getISPPf(tmpf);
 238         RRTNode *cc;
 239         if (this->slotSide() == LEFT)
 240                 cc = tmpb->ccl();
 241         else
 242                 cc = tmpb->ccr();
 243         if (this->slotSide() == LEFT)
 244                 this->DH(-1 * 0.5 / tmpb->out_radi());
 245         else
 246                 this->DH(1 * 0.5 / tmpb->out_radi());
 247         BicycleCar *p;
 248         int i = 1;
 249         p = tmpb->move(cc, i * this->DH());
 250         while (
 251                 !this->slot().collide(p->frame())
 252                 && ((
 253                         this->slotSide() == LEFT
 254                         && p->h() > this->slotHeading()
 255                 ) || (
 256                         this->slotSide() == RIGHT
 257                         && p->h() < this->slotHeading()
 258                 ))
 259         ) {
 260                 bool end = false;
 261                 std::vector<RRTEdge *> eds = p->frame();
 262                 for (auto o: co)
 263                         if (o.collide(eds))
 264                                 end = true;
 265                 for (auto o: so)
 266                         if (o.collide(eds))
 267                                 end = true;
 268                 for (auto e: eds)
 269                         delete e;
 270                 if (end)
 271                         break;
 272                 this->goals_.push_back(p);
 273                 tmpc.push_back(p);
 274                 i += 1;
 275                 p = tmpb->move(cc, i * this->DH());
 276         }
 277         if (tmpc.size() > 0)
 278                 this->cusp().push_back(tmpc);
 279         return;
 280 }
 281
 282 BicycleCar *ParallelSlot::flnc(
 283         BicycleCar *B,
 284         std::vector<CircleObstacle>& co,
 285         std::vector<SegmentObstacle>& so
 286 )
 287 {
 288         RRTNode *cc;
 289         if (this->slotSide() == LEFT) {
 290                 if (int(B->s()) % 2 == 0)
 291                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 292                 else
 293                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 294         } else {
 295                 if (int(B->s()) % 2 == 0)
 296                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 297                 else
 298                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 299         }
 300         BicycleCar *p;
 301         int i = 1;
 302         p = B->move(cc, i * this->DH());
 303         while (
 304                 !this->slot().collide(p->frame())
 305                 && std::abs(this->slotHeading() - p->h()) < M_PI / 2
 306         ) {
 307                 delete p;
 308                 i += 10;
 309                 p = B->move(cc, i * this->DH());
 310                 bool end = false;
 311                 std::vector<RRTEdge *> eds = p->frame();
 312                 for (auto o: co)
 313                         if (o.collide(eds))
 314                                 end = true;
 315                 for (auto o: so)
 316                         if (o.collide(eds))
 317                                 end = true;
 318                 for (auto e: eds)
 319                         delete e;
 320                 if (end)
 321                         break;
 322         }
 323         i -= 10;
 324         p = B->move(cc, i * this->DH());
 325         while (
 326                 !this->slot().collide(p->frame())
 327                 && std::abs(this->slotHeading() - p->h()) < M_PI / 2
 328         ) {
 329                 if (this->isInside(p)) {
 330                         i += 1;
 331                         break;
 332                 }
 333                 delete p;
 334                 i += 1;
 335                 p = B->move(cc, i * this->DH());
 336                 bool end = false;
 337                 std::vector<RRTEdge *> eds = p->frame();
 338                 for (auto o: co)
 339                         if (o.collide(eds))
 340                                 end = true;
 341                 for (auto o: so)
 342                         if (o.collide(eds))
 343                                 end = true;
 344                 for (auto e: eds)
 345                         delete e;
 346                 if (end)
 347                         break;
 348         }
 349         delete p;
 350         return B->move(cc, (i - 1) * this->DH());
 351 }
 352
 353 void ParallelSlot::fipr(RRTNode *n)
 354 {
 355         return this->fipr(new BicycleCar(n->x(), n->y(), n->h()));
 356 }
 357
 358 void ParallelSlot::fipr(BicycleCar *B)
 359 {
 360         std::vector<RRTNode *> cusp;
 361         cusp.push_back(new RRTNode(B->x(), B->y(), B->h()));
 362         int di = 1;
 363         if (this->slotSide() == LEFT)
 364                 di = -1;
 365         if (this->slotType() == PERPENDICULAR) {
 366                 this->DH(di * 0.01 / B->out_radi()); // TODO car in slot h()
 367                 RRTNode *cc;
 368                 if (this->slotSide() == LEFT)
 369                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 370                 else
 371                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 372                 BicycleCar *p;
 373                 int i = 1;
 374                 p = B->move(cc, i * this->DH());
 375                 while (
 376                         !this->slot().collide(p->frame())
 377                         && this->slot().collide(p)
 378                 ) {
 379                         delete p;
 380                         i += 10;
 381                         p = B->move(cc, i * this->DH());
 382                 }
 383                 i -= 10;
 384                 p = B->move(cc, i * this->DH());
 385                 while (
 386                         !this->slot().collide(p->frame())
 387                         && this->slot().collide(p)
 388                 ) {
 389                         delete p;
 390                         i += 1;
 391                         p = B->move(cc, i * this->DH());
 392                 }
 393                 i -= 1;
 394                 p = B->move(cc, i * this->DH());
 395                 cusp.push_back(new RRTNode(p->x(), p->y(), p->h()));
 396                 std::reverse(cusp.begin(), cusp.end());
 397                 this->cusp().push_back(cusp);
 398                 return;
 399         }
 400         this->DH(di * 0.01 / B->out_radi());
 401         BicycleCar *c;
 402         c = this->flncr(B);
 403         c->s(B->s() + 1);
 404         while ((
 405                 this->slotSide() == LEFT
 406                 && this->slot().collide(new RRTNode(c->lfx(), c->lfy(), 0))
 407         ) || (
 408                 this->slotSide() == RIGHT
 409                 && this->slot().collide(new RRTNode(c->rfx(), c->rfy(), 0))
 410         )) {
 411                 cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
 412                 BicycleCar *cc = this->flncr(c);
 413                 cc->s(c->s() + 1);
 414                 delete c;
 415                 c = cc;
 416         }
 417         cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
 418         std::reverse(cusp.begin(), cusp.end());
 419         this->cusp().push_back(cusp);
 420 }
 421
 422 BicycleCar *ParallelSlot::flncr(BicycleCar *B)
 423 {
 424         RRTNode *cc;
 425         if (this->slotSide() == LEFT) {
 426                 if (int(B->s()) % 2 == 0)
 427                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 428                 else
 429                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 430         } else {
 431                 if (int(B->s()) % 2 == 0)
 432                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 433                 else
 434                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 435         }
 436         BicycleCar *p;
 437         int i = 1;
 438         p = B->move(cc, i * this->DH());
 439         while (
 440                 !this->slot().collide(p->frame())
 441                 && ((
 442                         this->slotSide() == LEFT
 443                         && this->slot().collide(new RRTNode(
 444                                 p->lfx(),
 445                                 p->lfy(),
 446                                 0
 447                         ))
 448                 ) || (
 449                         this->slotSide() == RIGHT
 450                         && this->slot().collide(new RRTNode(
 451                                 p->rfx(),
 452                                 p->rfy(),
 453                                 0
 454                         ))
 455                 ))
 456         ) {
 457                 delete p;
 458                 i += 10;
 459                 p = B->move(cc, i * this->DH());
 460         }
 461         i -= 10;
 462         p = B->move(cc, i * this->DH());
 463         while (!this->slot().collide(p->frame())) {
 464                 if(
 465                         this->slotSide() == LEFT
 466                         && !this->slot().collide(new RRTNode(
 467                                 p->lfx(),
 468                                 p->lfy(),
 469                                 0
 470                         ))
 471                 ) {
 472                         i += 1;
 473                         break;
 474                 }
 475                 if(
 476                         this->slotSide() == RIGHT
 477                         && !this->slot().collide(new RRTNode(
 478                                 p->rfx(),
 479                                 p->rfy(),
 480                                 0
 481                         ))
 482                 ) {
 483                         i += 1;
 484                         break;
 485                 }
 486                 i += 1;
 487                 p = B->move(cc, i * this->DH());
 488         }
 489         delete p;
 490         return B->move(cc, (i - 1) * this->DH());
 491 }
 492
 493 RRTNode *ParallelSlot::fposecenter()
 494 {
 495         return this->slot().bnodes().back();
 496 }
 497
 498 bool ParallelSlot::flast(
 499         RRTNode *P,
 500         bool right,
 501         int il,
 502         std::vector<RRTNode *> &cusp
 503 )
 504 {
 505         BicycleCar *B = new BicycleCar(P->x(), P->y(), P->h());
 506         RRTNode *cc;
 507         if (right)
 508                 cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 509         else
 510                 cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 511         BicycleCar *p;
 512         int i = 1;
 513         p = B->move(cc, i * this->DH());
 514         while (!this->slot().collide(p->frame())
 515                         && (
 516                                 (this->DH() > 0 && p->x() <= 0)
 517                                 || (this->DH() < 0 && p->x() >= 0)
 518                         )) {
 519                 delete p;
 520                 i += 10;
 521                 p = B->move(cc, i * this->DH());
 522         }
 523         i -= 10;
 524         p = B->move(cc, i * this->DH());
 525         while (!this->slot().collide(p->frame())
 526                         && (
 527                                 (this->DH() > 0 && p->x() <= 0)
 528                                 || (this->DH() < 0 && p->x() >= 0)
 529                         )) {
 530                 if (this->DH() > 0 && p->rfx() <= 0 && p->rrx() <= 0) {
 531                         i += 1;
 532                         break;
 533                 }
 534                 if (this->DH() < 0 && p->lfx() >= 0 && p->lrx() >= 0) {
 535                         i += 1;
 536                         break;
 537                 }
 538                 delete p;
 539                 i += 1;
 540                 p = B->move(cc, i * this->DH());
 541         }
 542         delete p;
 543         p = B->move(cc, (i - 1) * this->DH());
 544         if (this->DH() > 0 && p->rfx() <= 0 && p->rrx() <= 0) {
 545                 cusp.push_back(p);
 546                 return true;
 547         } else if (this->DH() < 0 && p->lfx() >= 0 && p->lrx() >= 0) {
 548                 cusp.push_back(p);
 549                 return true;
 550         } else if (il < 8) {
 551                 cusp.push_back(p);
 552                 return this->flast(p, !right, il + 1, cusp);
 553         }
 554         return false;
 555 }
 556
 557 void ParallelSlot::fpose()
 558 {
 559         bool left = false; // right parking slot
 560         float di = -1;
 561         BicycleCar *CC = new BicycleCar(
 562                 this->fposecenter()->x(),
 563                 this->fposecenter()->y() - 0.01,
 564                 this->slotHeading()
 565         );
 566         BicycleCar *B = new BicycleCar(
 567                 CC->x() - CC->width() / 2,
 568                 CC->y() - (CC->length() + CC->wheelbase()) / 2,
 569                 this->slotHeading()
 570         );
 571         if (this->slot().bnodes()[0]->x() > this->slot().bnodes()[1]->x()) {
 572                 left = true;
 573                 di = 1;
 574                 delete B;
 575                 B = new BicycleCar(
 576                         CC->x() + CC->width() / 2,
 577                         CC->y() - (CC->length() + CC->wheelbase()) / 2,
 578                         this->slotHeading()
 579                 );
 580         }
 581         this->DH(di * 0.01 / CC->out_radi());
 582         BicycleCar *p;
 583         int i = 0;
 584         p = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 585         while (!this->slot().collide(p->frame())) {
 586                 std::vector<RRTNode *> tmpcusp;
 587                 tmpcusp.push_back(new BicycleCar(p->x(), p->y(), p->h()));
 588                 if (this->flast(p, left, 0, tmpcusp)) {
 589                         this->cusp().push_back(tmpcusp);
 590                         return;
 591                 }
 592                 i += 1;
 593                 delete p;
 594                 p = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 595         }
 596 }
 597
 598 BicycleCar *ParallelSlot::getEP()
 599 {
 600         // new pose for parallel parking to right slot
 601         float tnx;
 602         float tny;
 603         float nx;
 604         float ny;
 605         BicycleCar *CC = this->getEPC();
 606         // move left by car width / 2
 607         tnx = CC->x() + CC->width() / 2 * cos(CC->h() + M_PI / 2);
 608         tny = CC->y() + CC->width() / 2 * sin(CC->h() + M_PI / 2);
 609         if (this->slotSide() == LEFT) {
 610                 // move right by car width / 2
 611                 tnx = CC->x() + CC->width() / 2 * cos(CC->h() - M_PI / 2);
 612                 tny = CC->y() + CC->width() / 2 * sin(CC->h() - M_PI / 2);
 613         }
 614         if (this->slotType() == PARALLEL) {
 615                 // move down
 616                 nx = tnx - (CC->length() + CC->wheelbase()) / 2 * cos(CC->h());
 617                 ny = tny - (CC->length() + CC->wheelbase()) / 2 * sin(CC->h());
 618         } else {
 619                 // move down
 620                 nx = tnx + (CC->length() - CC->wheelbase()) / 2 * cos(CC->h());
 621                 ny = tny + (CC->length() - CC->wheelbase()) / 2 * sin(CC->h());
 622         }
 623         return new BicycleCar(nx, ny, CC->h());
 624 }
 625
 626 BicycleCar *ParallelSlot::getEPC()
 627 {
 628         // new pose for parallel parking to right slot
 629         float ta;
 630         float nx;
 631         float ny;
 632         ta = this->slotHeading() + M_PI;
 633         if (this->slotSide() == RIGHT)
 634                 ta -= M_PI / 4;
 635         else
 636                 ta += M_PI / 4;
 637         nx = this->fposecenter()->x() + 0.01 * cos(ta);
 638         ny = this->fposecenter()->y() + 0.01 * sin(ta);
 639         return new BicycleCar(nx, ny, this->slotHeading());
 640 }
 641
 642 BicycleCar *ParallelSlot::getFP()
 643 {
 644         this->setAll();
 645         float x = this->slot().bnodes()[0]->x();
 646         float y = this->slot().bnodes()[0]->y();
 647         float h = this->slotHeading();
 648         float nx;
 649         float ny;
 650         if (this->slotType() == PARALLEL) {
 651                 if (this->slotSide() == LEFT) {
 652                         nx = x + BCAR_WIDTH / 2 * cos(h + M_PI / 2);
 653                         ny = y + BCAR_WIDTH / 2 * sin(h + M_PI / 2);
 654                 } else {
 655                         nx = x + BCAR_WIDTH / 2 * cos(h - M_PI / 2);
 656                         ny = y + BCAR_WIDTH / 2 * sin(h - M_PI / 2);
 657                 }
 658                 x = nx + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * cos(h);
 659                 y = ny + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * sin(h);
 660         } else {
 661                 if (this->slotSide() == LEFT) {
 662                         h -= M_PI / 2;
 663                         nx = x + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
 664                                 * cos(h + M_PI);
 665                         ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
 666                                 * sin(h + M_PI);
 667                         x = nx + (BCAR_DIAG_RRADI) * cos(h + M_PI / 2);
 668                         y = ny + (BCAR_DIAG_RRADI) * sin(h + M_PI / 2);
 669                 } else {
 670                         h += M_PI / 2;
 671                         nx = x + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
 672                                 * cos(h - M_PI);
 673                         ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
 674                                 * sin(h - M_PI);
 675                         x = nx + (BCAR_DIAG_RRADI) * cos(h - M_PI / 2);
 676                         y = ny + (BCAR_DIAG_RRADI) * sin(h - M_PI / 2);
 677                 }
 678         }
 679         return new BicycleCar(x, y, h);
 680 }
 681
 682 BicycleCar *ParallelSlot::getISPP(BicycleCar *B)
 683 {
 684         float x = this->slot().bnodes().back()->x();
 685         float y = this->slot().bnodes().back()->y();
 686         float y0;
 687         if (this->slotSide() == LEFT) // TODO only for backward parking now
 688                 y0 = B->ccl()->y();
 689         else
 690                 y0 = B->ccr()->y();
 691         float IR = BCAR_IN_RADI;
 692         float a = 1;
 693         float b = -2 * x;
 694         float c = pow(x, 2) + pow(y - y0, 2) - pow(IR, 2);
 695         float D = pow(b, 2) - 4 * a * c;
 696         float x0;
 697         if (this->slotSide() == LEFT)
 698                 x0 = -b - sqrt(D);
 699         else
 700                 x0 = -b + sqrt(D);
 701         x0 /= 2 * a;
 702         return new BicycleCar(x0, B->y(), B->h());
 703 }
 704
 705 BicycleCar *ParallelSlot::getFPf()
 706 {
 707         this->setAll();
 708         float x = this->slot().bnodes().front()->x();
 709         float y = this->slot().bnodes().front()->y();
 710         float h = this->slotHeading();
 711         float ph = this->poseHeading();
 712         ph += M_PI;
 713         while (ph > M_PI)
 714                 ph -= 2 * M_PI;
 715         while (ph <= -M_PI)
 716                 ph += 2 * M_PI;
 717         float nx;
 718         float ny;
 719         nx = x + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 * cos(ph);
 720         ny = y + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 * sin(ph);
 721         x = nx + (BCAR_DIAG_RRADI) * cos(h);
 722         y = ny + (BCAR_DIAG_RRADI) * sin(h);
 723         return new BicycleCar(x, y, ph);
 724 }
 725
 726 BicycleCar *ParallelSlot::getISPPf(BicycleCar *B)
 727 {
 728         // right rear (for right parking slot)
 729         float x = this->slot().bnodes().front()->x();
 730         float y = this->slot().bnodes().front()->y();
 731         float x1;
 732         float y1;
 733         if (this->slotSide() == LEFT) {
 734                 x1 = B->ccl()->x();
 735                 y1 = B->ccl()->y();
 736         } else {
 737                 x1 = B->ccr()->x();
 738                 y1 = B->ccr()->y();
 739         }
 740         float IR = BCAR_IN_RADI;
 741         float a = 1;
 742         float b = (x - x1) * 2 * cos(B->h()) + (y - y1) * 2 * sin(B->h());
 743         float c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
 744         float D = pow(b, 2) - 4 * a * c;
 745         float delta;
 746         delta = -b - sqrt(D); // Use just `(-b - D) / (2a)` formula.
 747         delta /= 2 * a;
 748         float delta_1 = delta;
 749         // left front (for right parking slot)
 750         x = this->slot().bnodes().back()->x();
 751         y = this->slot().bnodes().back()->y();
 752         IR = BCAR_OUT_RADI;
 753         a = 1;
 754         b = (x - x1) * 2 * cos(B->h()) + (y - y1) * 2 * sin(B->h());
 755         c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
 756         D = pow(b, 2) - 4 * a * c;
 757         delta = -b + sqrt(D);
 758         delta /= 2 * a;
 759         float delta_2 = delta;
 760         delta = -b - sqrt(D);
 761         delta /= 2 * a;
 762         float delta_3 = delta;
 763         delta = std::max(delta_1, std::max(delta_2, delta_3));
 764         return new BicycleCar(
 765                 B->x() - delta * cos(B->h()),
 766                 B->y() - delta * sin(B->h()),
 767                 B->h()
 768         );
 769 }
 770
 771 bool ParallelSlot::isInside(BicycleCar *c)
 772 {
 773         bool inside = true;
 774         RRTNode *tmpn;
 775         tmpn = new RRTNode(c->lfx(), c->lfy(), 0);
 776         if (!this->slot().collide(tmpn))
 777                 inside = false;
 778         delete tmpn;
 779         tmpn = new RRTNode(c->lrx(), c->lry(), 0);
 780         if (!this->slot().collide(tmpn))
 781                 inside = false;
 782         delete tmpn;
 783         tmpn = new RRTNode(c->rrx(), c->rry(), 0);
 784         if (!this->slot().collide(tmpn))
 785                 inside = false;
 786         delete tmpn;
 787         tmpn = new RRTNode(c->rfx(), c->rfy(), 0);
 788         if (!this->slot().collide(tmpn))
 789                 inside = false;
 790         delete tmpn;
 791         return inside;
 792 }
 793
 794 struct SamplingInfo ParallelSlot::getSamplingInfo()
 795 {
 796         struct SamplingInfo si;
 797         RRTNode *n = this->getMidd();
 798         if (this->slotType() == PARALLEL) {
 799                 if (n != nullptr) {
 800                         si.x0 = n->x() + 1.5 * BCAR_LENGTH * cos(n->h());
 801                         si.y0 = n->y() + 1.5 * BCAR_LENGTH * sin(n->h());
 802                         si.h0 = n->h();
 803                 } else {
 804                         si.x0 = this->slot().bnodes().front()->x();
 805                         si.y0 = this->slot().bnodes().front()->y();
 806                         si.h0 = this->slotHeading();
 807                 }
 808                 si.x = BCAR_WIDTH;
 809                 si.y = BCAR_WIDTH;
 810                 si.h = M_PI / 8;
 811         } else {
 812                 // TODO
 813         }
 814         return si;
 815 }