Add more possible goals for perpendicular slot

diff --git a/src/psp.cc b/src/psp.cc
index c9a91408d684ff811a9683d0c98fa5b7c0894bbc..df6b58ea4468af08217f11082763db1297e134bc 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -115,8 +115,6 @@ void PSPlanner::guess_gc()
                         this->ps().x2() - this->ps().x1()
                 );
                 while (h < 0) h += 2 * M_PI;
-                x += 2 * cos(h);
-                y += 2 * sin(h);
 
                 //// This is for backward parking only.
                 //double entry_width = edist(
@@ -225,6 +223,22 @@ std::vector<BicycleCar> PSPlanner::possible_goals(
                         pi.push_back(BicycleCar(this->cc()));
                 }
                 this->cc() = BicycleCar(orig_cc);
+        } else {
+                if (!this->ps().right()) {
+                        this->cc().set_max_steer();
+                        for (unsigned int i = 0; i < cnt; i++) {
+                                this->cc().next();
+                                pi.push_back(BicycleCar(this->cc()));
+                        }
+                } else {
+                        this->cc().set_max_steer();
+                        this->cc().st(this->cc().st() * -1);
+                        for (unsigned int i = 0; i < cnt; i++) {
+                                this->cc().next();
+                                pi.push_back(BicycleCar(this->cc()));
+                        }
+                }
+                this->cc() = BicycleCar(orig_cc);
         }
         return pi;
 }