Merge branch 'hotfix/0.4.1'

[hubacji1/psp.git] / src / psp.cc

diff --git a/src/psp.cc b/src/psp.cc
index 6d8c0a14e07c79e5cadad599e0cc39ad77cdeecb..9d8ce2158300ca35f362e2b0f0a61a4dfe7a1492 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -107,8 +107,8 @@ void PSPlanner::guess_gc()
                         this->ps().x2() - this->ps().x1()
                 );
                 while (h < 0) h += 2 * M_PI;
-                x += this->gc().dr() * cos(h + M_PI);
-                y += this->gc().dr() * sin(h + M_PI);
+                x += 2 * this->gc().dr() * cos(h);
+                y += 2 * this->gc().dr() * sin(h);
 #else
                 // Backward parking
                 double entry_width = edist(
@@ -142,6 +142,7 @@ std::vector<BicycleCar> PSPlanner::last_maneuver()
                 // zig-zag out from the slot
                 this->cc() = BicycleCar(this->gc());
                 this->cc().sp(0.1);
+                lm.push_back(BicycleCar(this->cc()));
                 while (!this->left()) {
                         while (!this->collide() && !this->left()) {
                                 this->cc().next();
@@ -266,6 +267,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 +320,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);