Fix rotation of init for bfs paralell parking

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

diff --git a/src/psp.cc b/src/psp.cc
index 5b657cd638bc7344a670ed16acf68e6129e89b7a..9bcd7bb754771389bb558f198fca2c246c6d6183 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()
@@ -318,7 +322,7 @@ void PSPlanner::fe_parallel()
                 this->cc().rotate(
                         this->ps().x4(),
                         this->ps().y4() - 0.01,
-                        0.01
+                        ((this->ps().right()) ? 0.01 : -0.01)
                 );
         }
         // BFS - find entry current car `cc` and corresponding goal car `gc`