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 RRTNode *ParallelSlot::getMidd()
  29 {
  30         if (this->cusp().size() > 0)
  31                 return this->cusp().front().front();
  32         else
  33                 return nullptr;
  34 }
  35
  36 std::vector<std::vector<RRTNode *>> &ParallelSlot::cusp()
  37 {
  38         return this->cusp_;
  39 }
  40
  41 float ParallelSlot::DH() const
  42 {
  43         return this->DH_;
  44 }
  45
  46 PolygonObstacle &ParallelSlot::slot()
  47 {
  48         return this->slot_;
  49 }
  50
  51 float ParallelSlot::slotHeading()
  52 {
  53         return this->slotHeading_;
  54 }
  55
  56 SlotSide ParallelSlot::slotSide()
  57 {
  58         return this->slotSide_;
  59 }
  60
  61 SlotType ParallelSlot::slotType()
  62 {
  63         return this->slotType_;
  64 }
  65
  66 // setter
  67 void ParallelSlot::DH(float dh)
  68 {
  69         this->DH_ = dh;
  70 }
  71
  72 void ParallelSlot::setAll()
  73 {
  74         // slot heading
  75         float y0 = this->slot().bnodes()[0]->y();
  76         float x0 = this->slot().bnodes()[0]->x();
  77         float y3 = this->slot().bnodes()[3]->y();
  78         float x3 = this->slot().bnodes()[3]->x();
  79         float dy = y3 - y0;
  80         float dx = x3 - x0;
  81         this->slotHeading_ = atan2(dy, dx);
  82         // slot side
  83         float y1 = this->slot().bnodes()[1]->y();
  84         float x1 = this->slot().bnodes()[1]->x();
  85         if (sgn((x1 - x0) * (y3 - y0) - (y1 - y0) * (x3 - x0)) < 0)
  86                 this->slotSide_ = LEFT;
  87         else
  88                 this->slotSide_ = RIGHT;
  89         // slot type
  90         float d1 = EDIST(
  91                 this->slot().bnodes()[0],
  92                 this->slot().bnodes()[1]
  93         );
  94         float d2 = EDIST(
  95                 this->slot().bnodes()[1],
  96                 this->slot().bnodes()[2]
  97         );
  98         if (d1 > d2)
  99                 this->slotType_ = PERPENDICULAR;
 100         else
 101                 this->slotType_ = PARALLEL;
 102 }
 103
 104 // other
 105 void ParallelSlot::fip(
 106         std::vector<CircleObstacle>& co,
 107         std::vector<SegmentObstacle>& so
 108 )
 109 {
 110         this->setAll();
 111         if (this->slotType() == PERPENDICULAR) {
 112                 // TODO different slot headings
 113                 // this is jus for slot heading = pi / 2
 114                 this->DH(0.01 / BCAR_TURNING_RADIUS);
 115                 BicycleCar *perc = nullptr;
 116                 RRTNode *cc = nullptr;
 117                 BicycleCar *p = nullptr;
 118                 int i = 0;
 119                 float x;
 120                 float h;
 121                 float y;
 122                 // parking backward
 123                 x = this->slot().bnodes()[3]->x();
 124                 y = this->slot().bnodes()[3]->y();
 125                 h = 0;
 126                 // -> top
 127                 x -= BCAR_DIST_FRONT;
 128                 y = this->slot().bnodes()[3]->y();
 129                 y += BCAR_OUT_RRADI - BCAR_TURNING_RADIUS;
 130                 if (perc)
 131                         delete perc;
 132                 perc = new BicycleCar(x, y, h);
 133                 if (cc)
 134                         delete cc;
 135                 cc = perc->ccl();
 136                 if (p)
 137                         delete p;
 138                 p = perc->move(cc, i * this->DH());
 139                 while (p->x() < 0) {
 140                         delete p;
 141                         p = perc->move(cc, i * this->DH());
 142                         i++;
 143                 }
 144                 // -> bottom
 145                 // (reset for parking backward)
 146                 x = this->slot().bnodes()[0]->x();
 147                 y = this->slot().bnodes()[0]->y();
 148                 h = 0;
 149                 // -> bottom
 150                 x -= BCAR_DIST_FRONT;
 151                 // get y from quadratic equation
 152                 float tmpD = pow(-2 * this->slot().bnodes()[0]->y(), 2);
 153                 tmpD -= 4 * (
 154                         pow(x - this->slot().bnodes()[0]->x(), 2) +
 155                         pow(this->slot().bnodes()[0]->y(), 2) -
 156                         pow(BCAR_IN_RADI, 2)
 157                 );
 158                 y = 2 * this->slot().bnodes()[0]->y();
 159                 y += sqrt(tmpD);
 160                 y /= 2;
 161                 y -= BCAR_TURNING_RADIUS;
 162                 // -- end of quadratic equation
 163                 if (perc)
 164                         delete perc;
 165                 perc = new BicycleCar(x, y, h);
 166                 if (cc)
 167                         delete cc;
 168                 cc = perc->ccl();
 169                 if (p)
 170                         delete p;
 171                 p = perc->move(cc, i * this->DH());
 172                 while (p->x() < 0) {
 173                         delete p;
 174                         p = perc->move(cc, i * this->DH());
 175                         i++;
 176                 }
 177                 // store nodes
 178                 std::vector<RRTNode *> cusp;
 179                 cusp.push_back(new RRTNode(p->x(), p->y(), p->h()));
 180                 cusp.push_back(new RRTNode(x, y, h));
 181                 this->cusp().push_back(cusp);
 182                 return;
 183         }
 184         // see https://courses.cs.washington.edu/courses/cse326/03su/homework/hw3/bfs.html
 185         // RRTNode.s() works as iteration level
 186         std::queue<BicycleCar *, std::list<BicycleCar *>> q;
 187         std::queue<BicycleCar *, std::list<BicycleCar *>> empty;
 188         int di = -1;
 189         if (this->slotSide() == LEFT)
 190                 di = 1;
 191         BicycleCar *CC = this->getEPC();
 192         BicycleCar *B = this->getEP();
 193         this->DH(di * 0.01 / CC->out_radi());
 194         BicycleCar *c;
 195         int i = 0;
 196         c = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 197         while (!this->slot().collide(c->frame())) {
 198                 bool end = false;
 199                 std::vector<RRTEdge *> eds = c->frame();
 200                 for (auto o: co)
 201                         if (o.collide(eds))
 202                                 end = true;
 203                 for (auto o: so)
 204                         if (o.collide(eds))
 205                                 end = true;
 206                 for (auto e: eds)
 207                         delete e;
 208                 if (!end)
 209                         q.push(c);
 210                 c = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 211                 i += 1;
 212         }
 213         delete c; // not in q and collide
 214         while (!q.empty()) {
 215                 c = q.front();
 216                 q.pop();
 217                 if (this->isInside(c)) {
 218                         goto createcuspandfinish;
 219                 } else if (c->s() < 9) {
 220                         BicycleCar *cc = this->flnc(c, co, so);
 221                         cc->s(c->s() + 1);
 222                         cc->bcparent(c);
 223                         q.push(cc);
 224                 } else {
 225                         delete c; // not in q and collide
 226                 }
 227         }
 228         std::swap(q, empty);
 229         return;
 230 createcuspandfinish:
 231         std::vector<RRTNode *> cusp;
 232         while (c) {
 233                 cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
 234                 c = c->bcparent();
 235         }
 236         std::reverse(cusp.begin(), cusp.end());
 237         this->cusp().push_back(cusp);
 238         std::swap(q, empty);
 239 }
 240
 241 BicycleCar *ParallelSlot::flnc(
 242         BicycleCar *B,
 243         std::vector<CircleObstacle>& co,
 244         std::vector<SegmentObstacle>& so
 245 )
 246 {
 247         RRTNode *cc;
 248         if (this->slotSide() == LEFT) {
 249                 if (int(B->s()) % 2 == 0)
 250                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 251                 else
 252                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 253         } else {
 254                 if (int(B->s()) % 2 == 0)
 255                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 256                 else
 257                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 258         }
 259         BicycleCar *p;
 260         int i = 1;
 261         p = B->move(cc, i * this->DH());
 262         while (
 263                 !this->slot().collide(p->frame())
 264                 && std::abs(this->slotHeading() - p->h()) < M_PI / 2
 265         ) {
 266                 delete p;
 267                 i += 10;
 268                 p = B->move(cc, i * this->DH());
 269                 bool end = false;
 270                 std::vector<RRTEdge *> eds = p->frame();
 271                 for (auto o: co)
 272                         if (o.collide(eds))
 273                                 end = true;
 274                 for (auto o: so)
 275                         if (o.collide(eds))
 276                                 end = true;
 277                 for (auto e: eds)
 278                         delete e;
 279                 if (end)
 280                         break;
 281         }
 282         i -= 10;
 283         p = B->move(cc, i * this->DH());
 284         while (
 285                 !this->slot().collide(p->frame())
 286                 && std::abs(this->slotHeading() - p->h()) < M_PI / 2
 287         ) {
 288                 if (this->isInside(p)) {
 289                         i += 1;
 290                         break;
 291                 }
 292                 delete p;
 293                 i += 1;
 294                 p = B->move(cc, i * this->DH());
 295                 bool end = false;
 296                 std::vector<RRTEdge *> eds = p->frame();
 297                 for (auto o: co)
 298                         if (o.collide(eds))
 299                                 end = true;
 300                 for (auto o: so)
 301                         if (o.collide(eds))
 302                                 end = true;
 303                 for (auto e: eds)
 304                         delete e;
 305                 if (end)
 306                         break;
 307         }
 308         delete p;
 309         return B->move(cc, (i - 1) * this->DH());
 310 }
 311
 312 void ParallelSlot::fipr(RRTNode *n)
 313 {
 314         return this->fipr(new BicycleCar(n->x(), n->y(), n->h()));
 315 }
 316
 317 void ParallelSlot::fipr(BicycleCar *B)
 318 {
 319         std::vector<RRTNode *> cusp;
 320         cusp.push_back(new RRTNode(B->x(), B->y(), B->h()));
 321         int di = 1;
 322         if (this->slotSide() == LEFT)
 323                 di = -1;
 324         if (this->slotType() == PERPENDICULAR) {
 325                 this->DH(di * 0.01 / B->out_radi()); // TODO car in slot h()
 326                 RRTNode *cc;
 327                 if (this->slotSide() == LEFT)
 328                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 329                 else
 330                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 331                 BicycleCar *p;
 332                 int i = 1;
 333                 p = B->move(cc, i * this->DH());
 334                 while (
 335                         !this->slot().collide(p->frame())
 336                         && this->slot().collide(p)
 337                 ) {
 338                         delete p;
 339                         i += 10;
 340                         p = B->move(cc, i * this->DH());
 341                 }
 342                 i -= 10;
 343                 p = B->move(cc, i * this->DH());
 344                 while (
 345                         !this->slot().collide(p->frame())
 346                         && this->slot().collide(p)
 347                 ) {
 348                         delete p;
 349                         i += 1;
 350                         p = B->move(cc, i * this->DH());
 351                 }
 352                 i -= 1;
 353                 p = B->move(cc, i * this->DH());
 354                 cusp.push_back(new RRTNode(p->x(), p->y(), p->h()));
 355                 std::reverse(cusp.begin(), cusp.end());
 356                 this->cusp().push_back(cusp);
 357                 return;
 358         }
 359         this->DH(di * 0.01 / B->out_radi());
 360         BicycleCar *c;
 361         c = this->flncr(B);
 362         c->s(B->s() + 1);
 363         while ((
 364                 this->slotSide() == LEFT
 365                 && this->slot().collide(new RRTNode(c->lfx(), c->lfy(), 0))
 366         ) || (
 367                 this->slotSide() == RIGHT
 368                 && this->slot().collide(new RRTNode(c->rfx(), c->rfy(), 0))
 369         )) {
 370                 cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
 371                 BicycleCar *cc = this->flncr(c);
 372                 cc->s(c->s() + 1);
 373                 delete c;
 374                 c = cc;
 375         }
 376         cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
 377         std::reverse(cusp.begin(), cusp.end());
 378         this->cusp().push_back(cusp);
 379 }
 380
 381 BicycleCar *ParallelSlot::flncr(BicycleCar *B)
 382 {
 383         RRTNode *cc;
 384         if (this->slotSide() == LEFT) {
 385                 if (int(B->s()) % 2 == 0)
 386                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 387                 else
 388                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 389         } else {
 390                 if (int(B->s()) % 2 == 0)
 391                         cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 392                 else
 393                         cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 394         }
 395         BicycleCar *p;
 396         int i = 1;
 397         p = B->move(cc, i * this->DH());
 398         while (
 399                 !this->slot().collide(p->frame())
 400                 && ((
 401                         this->slotSide() == LEFT
 402                         && this->slot().collide(new RRTNode(
 403                                 p->lfx(),
 404                                 p->lfy(),
 405                                 0
 406                         ))
 407                 ) || (
 408                         this->slotSide() == RIGHT
 409                         && this->slot().collide(new RRTNode(
 410                                 p->rfx(),
 411                                 p->rfy(),
 412                                 0
 413                         ))
 414                 ))
 415         ) {
 416                 delete p;
 417                 i += 10;
 418                 p = B->move(cc, i * this->DH());
 419         }
 420         i -= 10;
 421         p = B->move(cc, i * this->DH());
 422         while (!this->slot().collide(p->frame())) {
 423                 if(
 424                         this->slotSide() == LEFT
 425                         && !this->slot().collide(new RRTNode(
 426                                 p->lfx(),
 427                                 p->lfy(),
 428                                 0
 429                         ))
 430                 ) {
 431                         i += 1;
 432                         break;
 433                 }
 434                 if(
 435                         this->slotSide() == RIGHT
 436                         && !this->slot().collide(new RRTNode(
 437                                 p->rfx(),
 438                                 p->rfy(),
 439                                 0
 440                         ))
 441                 ) {
 442                         i += 1;
 443                         break;
 444                 }
 445                 i += 1;
 446                 p = B->move(cc, i * this->DH());
 447         }
 448         delete p;
 449         return B->move(cc, (i - 1) * this->DH());
 450 }
 451
 452 RRTNode *ParallelSlot::fposecenter()
 453 {
 454         return this->slot().bnodes().back();
 455 }
 456
 457 bool ParallelSlot::flast(
 458         RRTNode *P,
 459         bool right,
 460         int il,
 461         std::vector<RRTNode *> &cusp
 462 )
 463 {
 464         BicycleCar *B = new BicycleCar(P->x(), P->y(), P->h());
 465         RRTNode *cc;
 466         if (right)
 467                 cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
 468         else
 469                 cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
 470         BicycleCar *p;
 471         int i = 1;
 472         p = B->move(cc, i * this->DH());
 473         while (!this->slot().collide(p->frame())
 474                         && (
 475                                 (this->DH() > 0 && p->x() <= 0)
 476                                 || (this->DH() < 0 && p->x() >= 0)
 477                         )) {
 478                 delete p;
 479                 i += 10;
 480                 p = B->move(cc, i * this->DH());
 481         }
 482         i -= 10;
 483         p = B->move(cc, i * this->DH());
 484         while (!this->slot().collide(p->frame())
 485                         && (
 486                                 (this->DH() > 0 && p->x() <= 0)
 487                                 || (this->DH() < 0 && p->x() >= 0)
 488                         )) {
 489                 if (this->DH() > 0 && p->rfx() <= 0 && p->rrx() <= 0) {
 490                         i += 1;
 491                         break;
 492                 }
 493                 if (this->DH() < 0 && p->lfx() >= 0 && p->lrx() >= 0) {
 494                         i += 1;
 495                         break;
 496                 }
 497                 delete p;
 498                 i += 1;
 499                 p = B->move(cc, i * this->DH());
 500         }
 501         delete p;
 502         p = B->move(cc, (i - 1) * this->DH());
 503         if (this->DH() > 0 && p->rfx() <= 0 && p->rrx() <= 0) {
 504                 cusp.push_back(p);
 505                 return true;
 506         } else if (this->DH() < 0 && p->lfx() >= 0 && p->lrx() >= 0) {
 507                 cusp.push_back(p);
 508                 return true;
 509         } else if (il < 8) {
 510                 cusp.push_back(p);
 511                 return this->flast(p, !right, il + 1, cusp);
 512         }
 513         return false;
 514 }
 515
 516 void ParallelSlot::fpose()
 517 {
 518         bool left = false; // right parking slot
 519         float di = -1;
 520         BicycleCar *CC = new BicycleCar(
 521                 this->fposecenter()->x(),
 522                 this->fposecenter()->y() - 0.01,
 523                 this->slotHeading()
 524         );
 525         BicycleCar *B = new BicycleCar(
 526                 CC->x() - CC->width() / 2,
 527                 CC->y() - (CC->length() + CC->wheelbase()) / 2,
 528                 this->slotHeading()
 529         );
 530         if (this->slot().bnodes()[0]->x() > this->slot().bnodes()[1]->x()) {
 531                 left = true;
 532                 di = 1;
 533                 delete B;
 534                 B = new BicycleCar(
 535                         CC->x() + CC->width() / 2,
 536                         CC->y() - (CC->length() + CC->wheelbase()) / 2,
 537                         this->slotHeading()
 538                 );
 539         }
 540         this->DH(di * 0.01 / CC->out_radi());
 541         BicycleCar *p;
 542         int i = 0;
 543         p = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 544         while (!this->slot().collide(p->frame())) {
 545                 std::vector<RRTNode *> tmpcusp;
 546                 tmpcusp.push_back(new BicycleCar(p->x(), p->y(), p->h()));
 547                 if (this->flast(p, left, 0, tmpcusp)) {
 548                         this->cusp().push_back(tmpcusp);
 549                         return;
 550                 }
 551                 i += 1;
 552                 delete p;
 553                 p = B->move(CC, -i * di * 0.01 / CC->diag_radi());
 554         }
 555 }
 556
 557 BicycleCar *ParallelSlot::getEP()
 558 {
 559         // new pose for parallel parking to right slot
 560         float tnx;
 561         float tny;
 562         float nx;
 563         float ny;
 564         BicycleCar *CC = this->getEPC();
 565         // move left by car width / 2
 566         tnx = CC->x() + CC->width() / 2 * cos(CC->h() + M_PI / 2);
 567         tny = CC->y() + CC->width() / 2 * sin(CC->h() + M_PI / 2);
 568         if (this->slotSide() == LEFT) {
 569                 // move right by car width / 2
 570                 tnx = CC->x() + CC->width() / 2 * cos(CC->h() - M_PI / 2);
 571                 tny = CC->y() + CC->width() / 2 * sin(CC->h() - M_PI / 2);
 572         }
 573         if (this->slotType() == PARALLEL) {
 574                 // move down
 575                 nx = tnx - (CC->length() + CC->wheelbase()) / 2 * cos(CC->h());
 576                 ny = tny - (CC->length() + CC->wheelbase()) / 2 * sin(CC->h());
 577         } else {
 578                 // move down
 579                 nx = tnx + (CC->length() - CC->wheelbase()) / 2 * cos(CC->h());
 580                 ny = tny + (CC->length() - CC->wheelbase()) / 2 * sin(CC->h());
 581         }
 582         return new BicycleCar(nx, ny, CC->h());
 583 }
 584
 585 BicycleCar *ParallelSlot::getEPC()
 586 {
 587         // new pose for parallel parking to right slot
 588         float ta;
 589         float nx;
 590         float ny;
 591         ta = this->slotHeading() + M_PI;
 592         if (this->slotSide() == RIGHT)
 593                 ta -= M_PI / 4;
 594         else
 595                 ta += M_PI / 4;
 596         nx = this->fposecenter()->x() + 0.01 * cos(ta);
 597         ny = this->fposecenter()->y() + 0.01 * sin(ta);
 598         return new BicycleCar(nx, ny, this->slotHeading());
 599 }
 600
 601 BicycleCar *ParallelSlot::getFP()
 602 {
 603         this->setAll();
 604         float x = this->slot().bnodes()[0]->x();
 605         float y = this->slot().bnodes()[0]->y();
 606         float h = this->slotHeading();
 607         float nx;
 608         float ny;
 609         if (this->slotType() == PARALLEL) {
 610                 if (this->slotSide() == LEFT) {
 611                         nx = x + BCAR_WIDTH / 2 * cos(h + M_PI / 2);
 612                         ny = y + BCAR_WIDTH / 2 * sin(h + M_PI / 2);
 613                 } else {
 614                         nx = x + BCAR_WIDTH / 2 * cos(h - M_PI / 2);
 615                         ny = y + BCAR_WIDTH / 2 * sin(h - M_PI / 2);
 616                 }
 617                 x = nx + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * cos(h);
 618                 y = ny + ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2 + 0.01) * sin(h);
 619         } else {
 620                 if (this->slotSide() == LEFT) {
 621                         h -= M_PI / 2;
 622                         nx = x + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
 623                                 * cos(h + M_PI);
 624                         ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
 625                                 * sin(h + M_PI);
 626                         x = nx + (BCAR_DIAG_RRADI) * cos(h + M_PI / 2);
 627                         y = ny + (BCAR_DIAG_RRADI) * sin(h + M_PI / 2);
 628                 } else {
 629                         h += M_PI / 2;
 630                         nx = x + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
 631                                 * cos(h - M_PI);
 632                         ny = y + (BCAR_LENGTH + BCAR_WHEEL_BASE) / 2
 633                                 * sin(h - M_PI);
 634                         x = nx + (BCAR_DIAG_RRADI) * cos(h - M_PI / 2);
 635                         y = ny + (BCAR_DIAG_RRADI) * sin(h - M_PI / 2);
 636                 }
 637         }
 638         return new BicycleCar(x, y, h);
 639 }
 640
 641 BicycleCar *ParallelSlot::getISPP(BicycleCar *B)
 642 {
 643         float x = this->slot().bnodes().back()->x();
 644         float y = this->slot().bnodes().back()->y();
 645         float y0;
 646         if (this->slotSide() == LEFT) // TODO only for backward parking now
 647                 y0 = B->ccl()->y();
 648         else
 649                 y0 = B->ccr()->y();
 650         float IR = BCAR_IN_RADI;
 651         float a = 1;
 652         float b = -2 * x;
 653         float c = pow(x, 2) + pow(y - y0, 2) - pow(IR, 2);
 654         float D = pow(b, 2) - 4 * a * c;
 655         float x0 = -b + sqrt(D);
 656         x0 /= 2 * a;
 657         return new BicycleCar(x0, B->y(), B->h());
 658 }
 659
 660 bool ParallelSlot::isInside(BicycleCar *c)
 661 {
 662         bool inside = true;
 663         RRTNode *tmpn;
 664         tmpn = new RRTNode(c->lfx(), c->lfy(), 0);
 665         if (!this->slot().collide(tmpn))
 666                 inside = false;
 667         delete tmpn;
 668         tmpn = new RRTNode(c->lrx(), c->lry(), 0);
 669         if (!this->slot().collide(tmpn))
 670                 inside = false;
 671         delete tmpn;
 672         tmpn = new RRTNode(c->rrx(), c->rry(), 0);
 673         if (!this->slot().collide(tmpn))
 674                 inside = false;
 675         delete tmpn;
 676         tmpn = new RRTNode(c->rfx(), c->rfy(), 0);
 677         if (!this->slot().collide(tmpn))
 678                 inside = false;
 679         delete tmpn;
 680         return inside;
 681 }
 682
 683 struct SamplingInfo ParallelSlot::getSamplingInfo()
 684 {
 685         struct SamplingInfo si;
 686         RRTNode *n = this->getMidd();
 687         if (this->slotType() == PARALLEL) {
 688                 if (n != nullptr) {
 689                         si.x0 = n->x() + 1.5 * BCAR_LENGTH * cos(n->h());
 690                         si.y0 = n->y() + 1.5 * BCAR_LENGTH * sin(n->h());
 691                         si.h0 = n->h();
 692                 } else {
 693                         si.x0 = this->slot().bnodes().front()->x();
 694                         si.y0 = this->slot().bnodes().front()->y();
 695                         si.h0 = this->slotHeading();
 696                 }
 697                 si.x = BCAR_WIDTH;
 698                 si.y = BCAR_WIDTH;
 699                 si.h = M_PI / 8;
 700         } else {
 701                 // TODO
 702         }
 703         return si;
 704 }