Add shrink to perfect length method

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 138bcb1ac4ed763d44056d55e82ad0a98e7189b7..31adcac91c28daa8c32a1ae5e0b008d62fbe4602 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,7 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - Workaround to not working `fe()` for perpendicular parking slot.
 - Return maneuver as vector of BicycleCars.
 - `FORWARD_PARKING` macro.
+- Shrink to perfect length when too big parking slot.
 
 ### Fixed
 - Possible goals for right parallel parking slot.

diff --git a/api/psp.h b/api/psp.h
index 2294a5b3f4175539062ebee59cd01b86ecc634f4..70bbf79c42ee500131942f8e196f1e00ec6ad1e4 100644 (file)
--- a/api/psp.h
+++ b/api/psp.h
@@ -90,6 +90,15 @@ class PSPlanner {
                 {
                         return this->possible_goals(10, 0.5);
                 }
+                /*! \brief Shrink parking slot to perfect length.
+
+                Based on goal car dimensions, shrink `p1` and `p2`
+                towards the `p3` and `p4`. (Because backward parallel
+                parking).
+
+                NOTE: This function works for parallel parking only.
+                */
+                void shrink_to_perfect_len();
 
                 // find entry
                 /*! \brief Find entry to the parking slot.

diff --git a/src/psp.cc b/src/psp.cc
index 6d8c0a14e07c79e5cadad599e0cc39ad77cdeecb..9021d260bf69e3f2fa5dcfb0877a7da164276bab 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -266,6 +266,29 @@ std::vector<BicycleCar> PSPlanner::possible_goals(
         return pi;
 }
 
+void PSPlanner::shrink_to_perfect_len()
+{
+        if (!this->ps().parallel())
+                return;
+        double perfect_len = this->gc().perfect_parking_slot_len();
+        if (edist(
+                this->ps().x1(), this->ps().y1(),
+                this->ps().x4(), this->ps().y4()
+        ) < perfect_len)
+                return;
+        double h = this->ps().heading();
+        h -= M_PI;
+        while (h < 0) h += 2 * M_PI;
+        double ch = perfect_len * cos(h);
+        double sh = perfect_len * sin(h);
+        this->ps().border(
+                this->ps().x4() + ch, this->ps().y4() + sh,
+                this->ps().x3() + ch, this->ps().y3() + sh,
+                this->ps().x3(), this->ps().y3(),
+                this->ps().x4(), this->ps().y4()
+        );
+}
+
 // find entry
 void PSPlanner::fe()
 {
@@ -296,6 +319,7 @@ double angle_between_closer_point(
 
 void PSPlanner::fe_parallel()
 {
+        this->shrink_to_perfect_len();
         BicycleCar bco = BicycleCar(this->gc());
         this->cc() = BicycleCar();
         this->cc().sp(-0.01);