Merge branch 'hotfix/0.4.1'

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

diff --git a/src/psp.cc b/src/psp.cc
index 9021d260bf69e3f2fa5dcfb0877a7da164276bab..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();