From ad65215575e3d4e792e566b6a71a84eadbb55436 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 4 Feb 2020 10:49:14 +0100
Subject: [PATCH] Fix rotation of init for bfs paralell parking

---
 src/psp.cc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/psp.cc b/src/psp.cc
index f8b5950..9bcd7bb 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -322,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`
-- 
2.39.2