Update find last not colliding

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 6b378cb883dedcde3d03b546330587529a69aa50..7b804c7cb9c79c41b46bff833e979d01ff28eea8 100644 (file)
--- 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;
                 }