Use reverse perpendicular in find entry procedure

diff --git a/src/psp.cc b/src/psp.cc
index 5b657cd638bc7344a670ed16acf68e6129e89b7a..f8b5950ade9ab744c452dcc9bb43288204e0b958 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -273,10 +273,14 @@ std::vector<BicycleCar> PSPlanner::possible_goals(
 // find entry
 void PSPlanner::fe()
 {
-        if (this->ps().parallel())
+        this->c_ = 0;
+        if (this->ps().parallel()) {
                 return this->fe_parallel();
-        else
-                return this->fe_perpendicular();
+        } else {
+                this->guess_gc();
+                this->cc() = BicycleCar(this->gc());
+                return this->fer_perpendicular();
+        }
 }
 
 void PSPlanner::fe_parallel()