From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 20 Jul 2020 15:07:31 +0000 (+0200)
Subject: Add shrink to perfect length method
X-Git-Tag: v0.4.0~2^2
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/commitdiff_plain/3231095bbfd687390fb681dc60ea4d9022cca054

Add shrink to perfect length method
---

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 138bcb1..31adcac 100644
--- 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 2294a5b..70bbf79 100644
--- 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 6d8c0a1..9021d26 100644
--- 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);