From d0e946109e4b9a27a0374c4634e6c2b6547c9fd6 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 27 May 2019 18:54:24 +0200
Subject: [PATCH] Update in slot pose for angle forward parking

---
 decision_control/slotplanner.cc | 59 +++++++++++++++++----------------
 1 file changed, 31 insertions(+), 28 deletions(-)

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index e949ad4..df09cfb 100644
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -725,44 +725,47 @@ BicycleCar *ParallelSlot::getFPf()
 
 BicycleCar *ParallelSlot::getISPPf(BicycleCar *B)
 {
+        // right rear (for right parking slot)
         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 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 = (x - x1) * 2 * cos(B->h()) + (y - y1) * 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;
-        float x0_1 = x0;
-        // left front
+        float delta;
+        delta = -b - sqrt(D); // Use just `(-b - D) / (2a)` formula.
+        delta /= 2 * a;
+        float delta_1 = delta;
+        // left front (for right parking slot)
         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);
+        b = (x - x1) * 2 * cos(B->h()) + (y - y1) * 2 * sin(B->h());
+        c = pow(x - x1, 2) + pow(y - y1, 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());
+        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()
+        );
 }
 
 bool ParallelSlot::isInside(BicycleCar *c)
-- 
2.39.2