From e2b90ff7829f75897de1651c0714215745cfc2da Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Sun, 10 Mar 2019 18:08:49 +0100 Subject: [PATCH] Update find last not colliding --- decision_control/slotplanner.cc | 30 +++++++++++------------------- 1 file changed, 11 insertions(+), 19 deletions(-) diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc index 6b378cb..7b804c7 100644 --- a/decision_control/slotplanner.cc +++ b/decision_control/slotplanner.cc @@ -203,34 +203,26 @@ BicycleCar *ParallelSlot::flnc(BicycleCar *B) // TODO find last not colliding // for now just copy flast() RRTNode *cc; - if (int(B->s()) % 2 == 0) + if (int(B->s()) % 2 == 0) { cc = BicycleCar(B->x(), B->y(), B->h()).ccr(); - else + } else { cc = BicycleCar(B->x(), B->y(), B->h()).ccl(); + } + float di = -1; + if (this->slotSide() == LEFT) + di = 1; BicycleCar *p; int i = 1; - p = B->move(cc, i * this->DH()); - while (!this->slot().collide(p->frame()) - && ( - (this->DH() > 0 && p->x() <= 0) - || (this->DH() < 0 && p->x() >= 0) - )) { + p = B->move(cc, i * di * this->DH()); + while (!this->slot().collide(p->frame())) { delete p; i += 10; p = B->move(cc, i * this->DH()); } i -= 10; - p = B->move(cc, i * this->DH()); - while (!this->slot().collide(p->frame()) - && ( - (this->DH() > 0 && p->x() <= 0) - || (this->DH() < 0 && p->x() >= 0) - )) { - if (this->DH() > 0 && p->rfx() <= 0 && p->rrx() <= 0) { - i += 1; - break; - } - if (this->DH() < 0 && p->lfx() >= 0 && p->lrx() >= 0) { + p = B->move(cc, i * di * this->DH()); + while (!this->slot().collide(p->frame())) { + if (this->isInside(p)) { i += 1; break; } -- 2.39.2