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                                 && (
 149                                         p->h() > this->slotHeading()
 150                                         || p->h() < 0
 151                                 )
 152                         ))
 153                 ) {
 154                         bool end = false;
 155                         std::vector<RRTEdge *> eds = p->frame();
 156                         for (auto o: co)
 157                                 if (o.collide(eds))
 158                                         end = true;
 159                         for (auto o: so)
 160                                 if (o.collide(eds))
 161                                         end = true;
 162                         for (auto e: eds)
 163                                 delete e;
 164                         if (end)
 165                                 break;
 166                         this->goals_.push_back(p);
 167                         tmpc.push_back(p);
 168                         i += 1;
 169                         p = tmpb->move(cc, i * this->DH());
 170                 }
 171                 if (tmpc.size() > 0)
 172                         this->cusp().push_back(tmpc);
 173                 return;
 174         }
 175         // see https://courses.cs.washington.edu/courses/cse326/03su/homework/hw3/bfs.html
 176         // RRTNode.s() works as iteration level
 177         std::queue<BicycleCar *, std::list<BicycleCar *>> q;
 178         std::queue<BicycleCar *, std::list<BicycleCar *>> empty;
 179         int di = -1;
 180         if (this->slotSide() == LEFT)
 181                 di = 1;
 182         BicycleCar *CC = this->getEPC();
 183         BicycleCar *B = this->getEP();
 184         this->DH(di * 0.01 / CC->out_radi());
 185         BicycleCar *c;
 186         int i = 0;
 187         c = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 188         while (!this->slot().collide(c->frame())) {
 189                 bool end = false;
 190                 std::vector<RRTEdge *> eds = c->frame();
 191                 for (auto o: co)
 192                         if (o.collide(eds))
 193                                 end = true;
 194                 for (auto o: so)
 195                         if (o.collide(eds))
 196                                 end = true;
 197                 for (auto e: eds)
 198                         delete e;
 199                 if (!end)
 200                         q.push(c);
 201                 c = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 202                 i += 1;
 203         }
 204         delete c; // not in q and collide
 205         while (!q.empty()) {
 206                 c = q.front();
 207                 q.pop();
 208                 if (this->isInside(c)) {
 209                         goto createcuspandfinish;
 210                 } else if (c->s() < 9) {
 211                         BicycleCar *cc = this->flnc(c, co, so);
 212                         cc->s(c->s() + 1);
 213                         cc->bcparent(c);
 214                         q.push(cc);
 215                 } else {
 216                         delete c; // not in q and collide
 217                 }
 218         }
 219         std::swap(q, empty);
 220         return;
 221 createcuspandfinish:
 222         std::vector<RRTNode *> cusp;
 223         while (c) {
 224                 cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
 225                 c = c->bcparent();
 226         }
 227         std::reverse(cusp.begin(), cusp.end());
 228         this->cusp().push_back(cusp);
 229         std::swap(q, empty);
 230 }
 231
 232 void ParallelSlot::fipf(
 233         std::vector<CircleObstacle>& co,
 234         std::vector<SegmentObstacle>& so
 235 )
 236 {
 237         this->setAll();
 238         std::vector<RRTNode *> tmpc;
 239         BicycleCar *tmpf = this->getFPf();
 240         BicycleCar *tmpb = this->getISPPf(tmpf);
 241         RRTNode *cc;
 242         if (this->slotSide() == LEFT)
 243                 cc = tmpb->ccl();
 244         else
 245                 cc = tmpb->ccr();
 246         if (this->slotSide() == LEFT)
 247                 this->DH(-1 * 0.5 / tmpb->out_radi());
 248         else
 249                 this->DH(1 * 0.5 / tmpb->out_radi());
 250         BicycleCar *p;
 251         int i = 1;
 252         p = tmpb->move(cc, i * this->DH());
 253         while (
 254                 !this->slot().collide(p->frame())
 255                 && ((
 256                         this->slotSide() == LEFT
 257                         && p->h() > this->slotHeading()
 258                 ) || (
 259                         this->slotSide() == RIGHT
 260                         && p->h() < this->slotHeading()
 261                 ))
 262         ) {
 263                 bool end = false;
 264                 std::vector<RRTEdge *> eds = p->frame();
 265                 for (auto o: co)
 266                         if (o.collide(eds))
 267                                 end = true;
 268                 for (auto o: so)
 269                         if (o.collide(eds))
 270                                 end = true;
 271                 for (auto e: eds)
 272                         delete e;
 273                 if (end)
 274                         break;
 275                 this->goals_.push_back(p);
 276                 tmpc.push_back(p);
 277                 i += 1;
 278                 p = tmpb->move(cc, i * this->DH());
 279         }
 280         if (tmpc.size() > 0)
 281                 this->cusp().push_back(tmpc);
 282         return;
 283 }
 284
 285 BicycleCar *ParallelSlot::flnc(
 286         BicycleCar *B,
 287         std::vector<CircleObstacle>& co,
 288         std::vector<SegmentObstacle>& so
 289 )
 290 {
 291         RRTNode *cc;
 292         if (this->slotSide() == LEFT) {
 293                 if (int(B->s()) % 2 == 0)
 294                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 295                 else
 296                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 297         } else {
 298                 if (int(B->s()) % 2 == 0)
 299                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 300                 else
 301                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 302         }
 303         BicycleCar *p;
 304         int i = 1;
 305         p = B->move(cc, i * this->DH());
 306         while (
 307                 !this->slot().collide(p->frame())
 308                 && std::abs(this->slotHeading() - p->h()) < M_PI / 2
 309         ) {
 310                 delete p;
 311                 i += 10;
 312                 p = B->move(cc, i * this->DH());
 313                 bool end = false;
 314                 std::vector<RRTEdge *> eds = p->frame();
 315                 for (auto o: co)
 316                         if (o.collide(eds))
 317                                 end = true;
 318                 for (auto o: so)
 319                         if (o.collide(eds))
 320                                 end = true;
 321                 for (auto e: eds)
 322                         delete e;
 323                 if (end)
 324                         break;
 325         }
 326         i -= 10;
 327         p = B->move(cc, i * this->DH());
 328         while (
 329                 !this->slot().collide(p->frame())
 330                 && std::abs(this->slotHeading() - p->h()) < M_PI / 2
 331         ) {
 332                 if (this->isInside(p)) {
 333                         i += 1;
 334                         break;
 335                 }
 336                 delete p;
 337                 i += 1;
 338                 p = B->move(cc, i * this->DH());
 339                 bool end = false;
 340                 std::vector<RRTEdge *> eds = p->frame();
 341                 for (auto o: co)
 342                         if (o.collide(eds))
 343                                 end = true;
 344                 for (auto o: so)
 345                         if (o.collide(eds))
 346                                 end = true;
 347                 for (auto e: eds)
 348                         delete e;
 349                 if (end)
 350                         break;
 351         }
 352         delete p;
 353         return B->move(cc, (i - 1) * this->DH());
 354 }
 355
 356 void ParallelSlot::fipr(RRTNode *n)
 357 {
 358         return this->fipr(new BicycleCar(n->x(), n->y(), n->h()));
 359 }
 360
 361 void ParallelSlot::fipr(BicycleCar *B)
 362 {
 363         std::vector<RRTNode *> cusp;
 364         cusp.push_back(new RRTNode(B->x(), B->y(), B->h()));
 365         int di = 1;
 366         if (this->slotSide() == LEFT)
 367                 di = -1;
 368         if (this->slotType() == PERPENDICULAR) {
 369                 this->DH(di * 0.01 / B->out_radi()); // TODO car in slot h()
 370                 RRTNode *cc;
 371                 if (this->slotSide() == LEFT)
 372                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 373                 else
 374                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 375                 BicycleCar *p;
 376                 int i = 1;
 377                 p = B->move(cc, i * this->DH());
 378                 while (
 379                         !this->slot().collide(p->frame())
 380                         && this->slot().collide(p)
 381                 ) {
 382                         delete p;
 383                         i += 10;
 384                         p = B->move(cc, i * this->DH());
 385                 }
 386                 i -= 10;
 387                 p = B->move(cc, i * this->DH());
 388                 while (
 389                         !this->slot().collide(p->frame())
 390                         && this->slot().collide(p)
 391                 ) {
 392                         delete p;
 393                         i += 1;
 394                         p = B->move(cc, i * this->DH());
 395                 }
 396                 i -= 1;
 397                 p = B->move(cc, i * this->DH());
 398                 cusp.push_back(new RRTNode(p->x(), p->y(), p->h()));
 399                 std::reverse(cusp.begin(), cusp.end());
 400                 this->cusp().push_back(cusp);
 401                 return;
 402         }
 403         this->DH(di * 0.01 / B->out_radi());
 404         BicycleCar *c;
 405         c = this->flncr(B);
 406         c->s(B->s() + 1);
 407         while ((
 408                 this->slotSide() == LEFT
 409                 && this->slot().collide(new RRTNode(c->lfx(), c->lfy(), 0))
 410         ) || (
 411                 this->slotSide() == RIGHT
 412                 && this->slot().collide(new RRTNode(c->rfx(), c->rfy(), 0))
 413         )) {
 414                 cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
 415                 BicycleCar *cc = this->flncr(c);
 416                 cc->s(c->s() + 1);
 417                 delete c;
 418                 c = cc;
 419         }
 420         cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
 421         std::reverse(cusp.begin(), cusp.end());
 422         this->cusp().push_back(cusp);
 423 }
 424
 425 BicycleCar *ParallelSlot::flncr(BicycleCar *B)
 426 {
 427         RRTNode *cc;
 428         if (this->slotSide() == LEFT) {
 429                 if (int(B->s()) % 2 == 0)
 430                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 431                 else
 432                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 433         } else {
 434                 if (int(B->s()) % 2 == 0)
 435                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 436                 else
 437                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 438         }
 439         BicycleCar *p;
 440         int i = 1;
 441         p = B->move(cc, i * this->DH());
 442         while (
 443                 !this->slot().collide(p->frame())
 444                 && ((
 445                         this->slotSide() == LEFT
 446                         && this->slot().collide(new RRTNode(
 447                                 p->lfx(),
 448                                 p->lfy(),
 449                                 0
 450                         ))
 451                 ) || (
 452                         this->slotSide() == RIGHT
 453                         && this->slot().collide(new RRTNode(
 454                                 p->rfx(),
 455                                 p->rfy(),
 456                                 0
 457                         ))
 458                 ))
 459         ) {
 460                 delete p;
 461                 i += 10;
 462                 p = B->move(cc, i * this->DH());
 463         }
 464         i -= 10;
 465         p = B->move(cc, i * this->DH());
 466         while (!this->slot().collide(p->frame())) {
 467                 if(
 468                         this->slotSide() == LEFT
 469                         && !this->slot().collide(new RRTNode(
 470                                 p->lfx(),
 471                                 p->lfy(),
 472                                 0
 473                         ))
 474                 ) {
 475                         i += 1;
 476                         break;
 477                 }
 478                 if(
 479                         this->slotSide() == RIGHT
 480                         && !this->slot().collide(new RRTNode(
 481                                 p->rfx(),
 482                                 p->rfy(),
 483                                 0
 484                         ))
 485                 ) {
 486                         i += 1;
 487                         break;
 488                 }
 489                 i += 1;
 490                 p = B->move(cc, i * this->DH());
 491         }
 492         delete p;
 493         return B->move(cc, (i - 1) * this->DH());
 494 }
 495
 496 RRTNode *ParallelSlot::fposecenter()
 497 {
 498         return this->slot().bnodes().back();
 499 }
 500
 501 bool ParallelSlot::flast(
 502         RRTNode *P,
 503         bool right,
 504         int il,
 505         std::vector<RRTNode *> &cusp
 506 )
 507 {
 508         BicycleCar *B = new BicycleCar(P->x(), P->y(), P->h());
 509         RRTNode *cc;
 510         if (right)
 511                 cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 512         else
 513                 cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 514         BicycleCar *p;
 515         int i = 1;
 516         p = B->move(cc, i * this->DH());
 517         while (!this->slot().collide(p->frame())
 518                         && (
 519                                 (this->DH() > 0 && p->x() <= 0)
 520                                 || (this->DH() < 0 && p->x() >= 0)
 521                         )) {
 522                 delete p;
 523                 i += 10;
 524                 p = B->move(cc, i * this->DH());
 525         }
 526         i -= 10;
 527         p = B->move(cc, i * this->DH());
 528         while (!this->slot().collide(p->frame())
 529                         && (
 530                                 (this->DH() > 0 && p->x() <= 0)
 531                                 || (this->DH() < 0 && p->x() >= 0)
 532                         )) {
 533                 if (this->DH() > 0 && p->rfx() <= 0 && p->rrx() <= 0) {
 534                         i += 1;
 535                         break;
 536                 }
 537                 if (this->DH() < 0 && p->lfx() >= 0 && p->lrx() >= 0) {
 538                         i += 1;
 539                         break;
 540                 }
 541                 delete p;
 542                 i += 1;
 543                 p = B->move(cc, i * this->DH());
 544         }
 545         delete p;
 546         p = B->move(cc, (i - 1) * this->DH());
 547         if (this->DH() > 0 && p->rfx() <= 0 && p->rrx() <= 0) {
 548                 cusp.push_back(p);
 549                 return true;
 550         } else if (this->DH() < 0 && p->lfx() >= 0 && p->lrx() >= 0) {
 551                 cusp.push_back(p);
 552                 return true;
 553         } else if (il < 8) {
 554                 cusp.push_back(p);
 555                 return this->flast(p, !right, il + 1, cusp);
 556         }
 557         return false;
 558 }
 559
 560 void ParallelSlot::fpose()
 561 {
 562         bool left = false; // right parking slot
 563         float di = -1;
 564         BicycleCar *CC = new BicycleCar(
 565                 this->fposecenter()->x(),
 566                 this->fposecenter()->y() - 0.01,
 567                 this->slotHeading()
 568         );
 569         BicycleCar *B = new BicycleCar(
 570                 CC->x() - CC->width() / 2,
 571                 CC->y() - (CC->length() + CC->wheelbase()) / 2,
 572                 this->slotHeading()
 573         );
 574         if (this->slot().bnodes()[0]->x() > this->slot().bnodes()[1]->x()) {
 575                 left = true;
 576                 di = 1;
 577                 delete B;
 578                 B = new BicycleCar(
 579                         CC->x() + CC->width() / 2,
 580                         CC->y() - (CC->length() + CC->wheelbase()) / 2,
 581                         this->slotHeading()
 582                 );
 583         }
 584         this->DH(di * 0.01 / CC->out_radi());
 585         BicycleCar *p;
 586         int i = 0;
 587         p = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 588         while (!this->slot().collide(p->frame())) {
 589                 std::vector<RRTNode *> tmpcusp;
 590                 tmpcusp.push_back(new BicycleCar(p->x(), p->y(), p->h()));
 591                 if (this->flast(p, left, 0, tmpcusp)) {
 592                         this->cusp().push_back(tmpcusp);
 593                         return;
 594                 }
 595                 i += 1;
 596                 delete p;
 597                 p = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 598         }
 599 }
 600
 601 BicycleCar *ParallelSlot::getEP()
 602 {
 603         // new pose for parallel parking to right slot
 604         float tnx;
 605         float tny;
 606         float nx;
 607         float ny;
 608         BicycleCar *CC = this->getEPC();
 609         // move left by car width / 2
 610         tnx = CC->x() + CC->width() / 2 * cos(CC->h() + M_PI / 2);
 611         tny = CC->y() + CC->width() / 2 * sin(CC->h() + M_PI / 2);
 612         if (this->slotSide() == LEFT) {
 613                 // move right by car width / 2
 614                 tnx = CC->x() + CC->width() / 2 * cos(CC->h() - M_PI / 2);
 615                 tny = CC->y() + CC->width() / 2 * sin(CC->h() - M_PI / 2);
 616         }
 617         if (this->slotType() == PARALLEL) {
 618                 // move down
 619                 nx = tnx - (CC->length() + CC->wheelbase()) / 2 * cos(CC->h());
 620                 ny = tny - (CC->length() + CC->wheelbase()) / 2 * sin(CC->h());
 621         } else {
 622                 // move down
 623                 nx = tnx + (CC->length() - CC->wheelbase()) / 2 * cos(CC->h());
 624                 ny = tny + (CC->length() - CC->wheelbase()) / 2 * sin(CC->h());
 625         }
 626         return new BicycleCar(nx, ny, CC->h());
 627 }
 628
 629 BicycleCar *ParallelSlot::getEPC()
 630 {
 631         // new pose for parallel parking to right slot
 632         float ta;
 633         float nx;
 634         float ny;
 635         ta = this->slotHeading() + M_PI;
 636         if (this->slotSide() == RIGHT)
 637                 ta -= M_PI / 4;
 638         else
 639                 ta += M_PI / 4;
 640         nx = this->fposecenter()->x() + 0.01 * cos(ta);
 641         ny = this->fposecenter()->y() + 0.01 * sin(ta);
 642         return new BicycleCar(nx, ny, this->slotHeading());
 643 }
 644
 645 BicycleCar *ParallelSlot::getFP()
 646 {
 647         this->setAll();
 648         float x = this->slot().bnodes()[0]->x();
 649         float y = this->slot().bnodes()[0]->y();
 650         float h = this->slotHeading();
 651         float ph = this->poseHeading();
 652         float nx;
 653         float ny;
 654         if (this->slotType() == PARALLEL) {
 655                 if (this->slotSide() == LEFT) {
 656                         nx = x + BCAR_WIDTH / 2 * cos(h + M_PI / 2);
 657                         ny = y + BCAR_WIDTH / 2 * sin(h + M_PI / 2);
 658                 } else {
 659                         nx = x + BCAR_WIDTH / 2 * cos(h - M_PI / 2);
 660                         ny = y + BCAR_WIDTH / 2 * sin(h - M_PI / 2);
 661                 }
 662                 x = nx + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * cos(h);
 663                 y = ny + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * sin(h);
 664         } else {
 665                 if (this->slotSide() == LEFT) {
 666                         nx = x + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
 667                                 * cos(ph + M_PI);
 668                         ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
 669                                 * sin(ph + M_PI);
 670                         x = nx + (BCAR_DIAG_RRADI) * cos(h);
 671                         y = ny + (BCAR_DIAG_RRADI) * sin(h);
 672                 } else {
 673                         nx = x + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
 674                                 * cos(ph + M_PI);
 675                         ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
 676                                 * sin(ph + M_PI);
 677                         x = nx + (BCAR_DIAG_RRADI) * cos(h);
 678                         y = ny + (BCAR_DIAG_RRADI) * sin(h);
 679                 }
 680         }
 681         return new BicycleCar(x, y, ph);
 682 }
 683
 684 BicycleCar *ParallelSlot::getISPP(BicycleCar *B)
 685 {
 686         // rigt side (for right parking slot)
 687         float x = this->slot().bnodes().back()->x();
 688         float y = this->slot().bnodes().back()->y();
 689         float x1;
 690         float y1;
 691         if (this->slotSide() == LEFT) {
 692                 x1 = B->ccl()->x();
 693                 y1 = B->ccl()->y();
 694         } else {
 695                 x1 = B->ccr()->x();
 696                 y1 = B->ccr()->y();
 697         }
 698         float IR = BCAR_IN_RADI;
 699         float a = 1;
 700         float b = (x1 - x) * 2 * cos(B->h()) + (y1 - y) * 2 * sin(B->h());
 701         float c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
 702         float D = pow(b, 2) - 4 * a * c;
 703         float delta;
 704         delta = -b - sqrt(D);
 705         delta /= 2 * a;
 706         float delta_1 = delta;
 707         // left front (for right parking slot)
 708         x = this->slot().bnodes().front()->x();
 709         y = this->slot().bnodes().front()->y();
 710         IR = BCAR_OUT_RADI;
 711         a = 1;
 712         b = (x1 - x) * 2 * cos(B->h()) + (y1 - y) * 2 * sin(B->h());
 713         c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
 714         D = pow(b, 2) - 4 * a * c;
 715         //delta = -b + sqrt(D);
 716         //delta /= 2 * a;
 717         float delta_2 = delta;
 718         delta = -b - sqrt(D);
 719         delta /= 2 * a;
 720         float delta_3 = delta;
 721         delta = std::max(delta_1, std::max(delta_2, delta_3));
 722         return new BicycleCar(
 723                 B->x() + delta * cos(B->h()),
 724                 B->y() + delta * sin(B->h()),
 725                 B->h()
 726         );
 727 }
 728
 729 BicycleCar *ParallelSlot::getFPf()
 730 {
 731         this->setAll();
 732         float x = this->slot().bnodes().front()->x();
 733         float y = this->slot().bnodes().front()->y();
 734         float h = this->slotHeading();
 735         float ph = this->poseHeading();
 736         ph += M_PI;
 737         while (ph > M_PI)
 738                 ph -= 2 * M_PI;
 739         while (ph <= -M_PI)
 740                 ph += 2 * M_PI;
 741         float nx;
 742         float ny;
 743         nx = x + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 * cos(ph);
 744         ny = y + (BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 * sin(ph);
 745         x = nx + (BCAR_DIAG_RRADI) * cos(h);
 746         y = ny + (BCAR_DIAG_RRADI) * sin(h);
 747         return new BicycleCar(x, y, ph);
 748 }
 749
 750 BicycleCar *ParallelSlot::getISPPf(BicycleCar *B)
 751 {
 752         // right rear (for right parking slot)
 753         float x = this->slot().bnodes().front()->x();
 754         float y = this->slot().bnodes().front()->y();
 755         float x1;
 756         float y1;
 757         if (this->slotSide() == LEFT) {
 758                 x1 = B->ccl()->x();
 759                 y1 = B->ccl()->y();
 760         } else {
 761                 x1 = B->ccr()->x();
 762                 y1 = B->ccr()->y();
 763         }
 764         float IR = BCAR_IN_RADI;
 765         float a = 1;
 766         float b = (x - x1) * 2 * cos(B->h()) + (y - y1) * 2 * sin(B->h());
 767         float c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
 768         float D = pow(b, 2) - 4 * a * c;
 769         float delta;
 770         delta = -b - sqrt(D); // TODO why this works?
 771         delta /= 2 * a;
 772         float delta_1 = delta;
 773         // left front (for right parking slot)
 774         x = this->slot().bnodes().back()->x();
 775         y = this->slot().bnodes().back()->y();
 776         IR = BCAR_OUT_RADI;
 777         a = 1;
 778         b = (x - x1) * 2 * cos(B->h()) + (y - y1) * 2 * sin(B->h());
 779         c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
 780         D = pow(b, 2) - 4 * a * c;
 781         delta = -b + sqrt(D);
 782         delta /= 2 * a;
 783         float delta_2 = delta;
 784         delta = -b - sqrt(D);
 785         delta /= 2 * a;
 786         float delta_3 = delta;
 787         delta = std::max(delta_1, std::max(delta_2, delta_3));
 788         return new BicycleCar(
 789                 B->x() - delta * cos(B->h()),
 790                 B->y() - delta * sin(B->h()),
 791                 B->h()
 792         );
 793 }
 794
 795 bool ParallelSlot::isInside(BicycleCar *c)
 796 {
 797         bool inside = true;
 798         RRTNode *tmpn;
 799         tmpn = new RRTNode(c->lfx(), c->lfy(), 0);
 800         if (!this->slot().collide(tmpn))
 801                 inside = false;
 802         delete tmpn;
 803         tmpn = new RRTNode(c->lrx(), c->lry(), 0);
 804         if (!this->slot().collide(tmpn))
 805                 inside = false;
 806         delete tmpn;
 807         tmpn = new RRTNode(c->rrx(), c->rry(), 0);
 808         if (!this->slot().collide(tmpn))
 809                 inside = false;
 810         delete tmpn;
 811         tmpn = new RRTNode(c->rfx(), c->rfy(), 0);
 812         if (!this->slot().collide(tmpn))
 813                 inside = false;
 814         delete tmpn;
 815         return inside;
 816 }
 817
 818 struct SamplingInfo ParallelSlot::getSamplingInfo()
 819 {
 820         struct SamplingInfo si;
 821         RRTNode *n = this->getMidd();
 822         if (this->slotType() == PARALLEL) {
 823                 if (n != nullptr) {
 824                         si.x0 = n->x() + 1.5 * BCAR_LENGTH * cos(n->h());
 825                         si.y0 = n->y() + 1.5 * BCAR_LENGTH * sin(n->h());
 826                         si.h0 = n->h();
 827                 } else {
 828                         si.x0 = this->slot().bnodes().front()->x();
 829                         si.y0 = this->slot().bnodes().front()->y();
 830                         si.h0 = this->slotHeading();
 831                 }
 832                 si.x = BCAR_WIDTH;
 833                 si.y = BCAR_WIDTH;
 834                 si.h = M_PI / 8;
 835         } else {
 836                 // TODO
 837         }
 838         return si;
 839 }