From e9bf64bc085ef60214011c1fb4e6072c35993a3a Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 27 May 2019 16:12:15 +0200
Subject: [PATCH] Add inside slot position for perp. forward parking

---
 decision_control/slotplanner.cc | 42 +++++++++++++++++++++++++++++++++
 incl/slotplanner.h              |  1 +
 2 files changed, 43 insertions(+)

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 1c32eb0..de4b4d5 100644
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -668,6 +668,48 @@ BicycleCar *ParallelSlot::getFPf()
         return new BicycleCar(x, y, h);
 }
 
+BicycleCar *ParallelSlot::getISPPf(BicycleCar *B)
+{
+        float x = this->slot().bnodes().front()->x();
+        float y = this->slot().bnodes().front()->y();
+        float y0;
+        if (this->slotSide() == LEFT)
+                y0 = B->ccl()->y();
+        else
+                y0 = B->ccr()->y();
+        float IR = BCAR_IN_RADI;
+        float a = 1;
+        float b = -2 * x;
+        float c = pow(x, 2) + pow(y - y0, 2) - pow(IR, 2);
+        float D = pow(b, 2) - 4 * a * c;
+        float x0;
+        if (this->slotSide() == LEFT)
+                x0 = -b - sqrt(D);
+        else
+                x0 = -b + sqrt(D);
+        x0 /= 2 * a;
+        float x0_1 = x0;
+        // left front
+        x = this->slot().bnodes().back()->x();
+        y = this->slot().bnodes().back()->y();
+        IR = BCAR_OUT_RADI;
+        a = 1;
+        b = -2 * x;
+        c = pow(x, 2) + pow(y - y0, 2) - pow(IR, 2);
+        D = pow(b, 2) - 4 * a * c;
+        if (this->slotSide() == LEFT)
+                x0 = -b + sqrt(D);
+        else
+                x0 = -b - sqrt(D);
+        x0 /= 2 * a;
+        float x0_2 = x0;
+        if (this->slotSide() == LEFT)
+                x0 = std::max(x0_1, x0_2);
+        else
+                x0 = std::min(x0_1, x0_2);
+        return new BicycleCar(x0, B->y(), B->h());
+}
+
 bool ParallelSlot::isInside(BicycleCar *c)
 {
         bool inside = true;
diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index ef539cd..d9a98ed 100644
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -121,6 +121,7 @@ class ParallelSlot {
                 @param B The pose to start from.
                 */
                 BicycleCar *getISPP(BicycleCar *B);
+                BicycleCar *getISPPf(BicycleCar *B); // perp. forward parking
                 /** Return true if car is inside slot */
                 bool isInside(BicycleCar *c);
                 /** Return values to set sampling function of RRT */
-- 
2.39.2