From a069f6e9645da2de1a5171bf1ca8b2c56033e759 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Fri, 7 Jun 2019 10:46:55 +0200
Subject: [PATCH] Update in slot pose for angle backward parking

---
 decision_control/slotplanner.cc | 50 ++++++++++++++++++++++++---------
 1 file changed, 36 insertions(+), 14 deletions(-)

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index a141c86..da3a193 100644
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -680,25 +680,47 @@ BicycleCar *ParallelSlot::getFP()
 
 BicycleCar *ParallelSlot::getISPP(BicycleCar *B)
 {
+        // rigt side (for right parking slot)
         float x = this->slot().bnodes().back()->x();
         float y = this->slot().bnodes().back()->y();
-        float y0;
-        if (this->slotSide() == LEFT) // TODO only for backward parking now
-                y0 = B->ccl()->y();
-        else
-                y0 = B->ccr()->y();
+        float x1;
+        float y1;
+        if (this->slotSide() == LEFT) {
+                x1 = B->ccl()->x();
+                y1 = B->ccl()->y();
+        } else {
+                x1 = B->ccr()->x();
+                y1 = 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 b = (x1 - x) * 2 * cos(B->h()) + (y1 - y) * 2 * sin(B->h());
+        float c = pow(x - x1, 2) + pow(y - y1, 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;
-        return new BicycleCar(x0, B->y(), B->h());
+        float delta;
+        delta = -b - sqrt(D);
+        delta /= 2 * a;
+        float delta_1 = delta;
+        // left front (for right parking slot)
+        x = this->slot().bnodes().front()->x();
+        y = this->slot().bnodes().front()->y();
+        IR = BCAR_OUT_RADI;
+        a = 1;
+        b = (x1 - x) * 2 * cos(B->h()) + (y1 - y) * 2 * sin(B->h());
+        c = pow(x - x1, 2) + pow(y - y1, 2) - pow(IR, 2);
+        D = pow(b, 2) - 4 * a * c;
+        //delta = -b + sqrt(D);
+        //delta /= 2 * a;
+        float delta_2 = delta;
+        delta = -b - sqrt(D);
+        delta /= 2 * a;
+        float delta_3 = delta;
+        delta = std::max(delta_1, std::max(delta_2, delta_3));
+        return new BicycleCar(
+                B->x() + delta * cos(B->h()),
+                B->y() + delta * sin(B->h()),
+                B->h()
+        );
 }
 
 BicycleCar *ParallelSlot::getFPf()
-- 
2.39.2