Change `fipr` method for perpendicular slot

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 819a44cfcaa226cb3be65b0c89d6bd7ca7b4cac5..5f9880b656b2541a36e04d03b47d792b228be895 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -152,11 +152,36 @@ void ParallelSlot::fipr(BicycleCar *B)
         if (this->slotSide() == LEFT)
                 di = -1;
         if (this->slotType() == PERPENDICULAR) {
-                cusp.push_back(new RRTNode(
-                        B->x() - di * B->length() * cos(B->h()),
-                        B->y() - di * B->length() * sin(B->h()),
-                        B->h()
-                ));
+                this->DH(di * 0.01 / B->out_radi()); // TODO car in slot h()
+                RRTNode *cc;
+                if (this->slotSide() == LEFT)
+                        cc = BicycleCar(B->x(), B->y(), B->h()).ccl();
+                else
+                        cc = BicycleCar(B->x(), B->y(), B->h()).ccr();
+                BicycleCar *p;
+                int i = 1;
+                p = B->move(cc, i * this->DH());
+                while (
+                        !this->slot().collide(p->frame())
+                        && this->slot().collide(p)
+                ) {
+                        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->slot().collide(p)
+                ) {
+                        delete p;
+                        i += 1;
+                        p = B->move(cc, i * this->DH());
+                }
+                i -= 1;
+                p = B->move(cc, i * this->DH());
+                cusp.push_back(new RRTNode(p->x(), p->y(), p->h()));
                 std::reverse(cusp.begin(), cusp.end());
                 this->cusp().push_back(cusp);
                 return;