From 1e54f83b2a117d6803b214df1bc7537ed603ccd9 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 3 Feb 2020 11:45:26 +0100
Subject: [PATCH] Refactor parallel bfs parking slot planner

---
 src/psp.cc | 91 ++++++++++++++++++++++--------------------------------
 1 file changed, 37 insertions(+), 54 deletions(-)

diff --git a/src/psp.cc b/src/psp.cc
index e39c2c3..9258012 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -281,66 +281,49 @@ void PSPlanner::fe()
 
 void PSPlanner::fe_parallel()
 {
-        // angle for distance from "entry" corner
-        double dist_angl = this->ps().heading() + M_PI;
-        dist_angl += (this->ps().right()) ? - M_PI / 4 : + M_PI / 4;
-        // set bicycle car `bci` basic dimensions and heading
-        BicycleCar bci = BicycleCar(this->gc());
         BicycleCar bco = BicycleCar(this->gc());
-        bci.h(this->ps().heading());
-        // move 0.01 from the "entry" corner
-        bci.x(this->ps().x4() + 0.01 * cos(dist_angl));
-        bci.y(this->ps().y4() + 0.01 * sin(dist_angl));
-        // align with parking "top" of slot (move backward)
-        dist_angl = bci.h() + M_PI;
-        bci.x(bci.x() + bci.df() * cos(dist_angl));
-        bci.y(bci.y() + bci.df() * sin(dist_angl));
-        // align with "entry" to pakring slot (move outside)
-        dist_angl = this->ps().heading();
-        dist_angl += (this->ps().right()) ? + M_PI / 2 : - M_PI / 2;
-        bci.x(bci.x() + bci.w() / 2 * cos(dist_angl));
-        bci.y(bci.y() + bci.w() / 2 * sin(dist_angl));
-        // set default speed, steer
-        bci.st(bci.wb() / bci.mtr());
+        this->cc() = BicycleCar();
+        this->cc().sp(-0.01);
+        this->cc().set_max_steer();
         if (!this->ps().right())
-                bci.st(bci.st() * -1);
-        bci.sp(-0.01);
-        // BFS - init all starts
-        // see https://courses.cs.washington.edu/courses/cse326/03su/homework/hw3/bfs.html
-        double dist_diag = sqrt(pow(bci.w() / 2, 2) + pow(bci.df(), 2));
-        if (this->ps().right())
-                dist_angl = atan2(bci.y() - bci.rfy(), bci.x() - bci.rfx());
-        else
-                dist_angl = atan2(bci.y() - bci.lfy(), bci.x() - bci.lfx());
-        double DIST_ANGL = dist_angl;
-        std::queue<BicycleCar, std::list<BicycleCar>> q;
-        while (
-                (
-                        this->ps().right()
-                        && dist_angl < DIST_ANGL + 3 * M_PI / 4
+                this->cc().st(this->cc().st() * -1);
+        this->cc().h(this->ps().heading());
+        double angl_in_slot = this->ps().heading() - M_PI / 4;
+        if (!this->ps().right())
+                angl_in_slot += M_PI / 2;
+        this->cc().x(
+                this->ps().x4()
+                + this->cc().w()/2 * cos(
+                        this->ps().heading()
+                        + (this->ps().right() ? + M_PI / 2 : - M_PI / 2)
                 )
-                || (
-                        !this->ps().right()
-                        && dist_angl > DIST_ANGL - 3 * M_PI / 4
+                + (this->cc().df() + 0.01) * cos(
+                        this->ps().heading() + M_PI
                 )
-        ) {
-                this->cc() = BicycleCar(bci);
-                if (this->ps().right()) {
-                        this->cc().x(bci.rfx() + dist_diag * cos(dist_angl));
-                        this->cc().y(bci.rfy() + dist_diag * sin(dist_angl));
-                } else {
-                        this->cc().x(bci.lfx() + dist_diag * cos(dist_angl));
-                        this->cc().y(bci.lfy() + dist_diag * sin(dist_angl));
-                }
-                this->cc().h(this->ps().heading() + dist_angl - DIST_ANGL);
-                if (!this->collide()) {
-                        q.push(BicycleCar(this->cc()));
-                }
-                dist_angl += (this->ps().right()) ? + 0.01 : - 0.01;
+        );
+        this->cc().y(
+                this->ps().y4()
+                + this->cc().w()/2 * sin(
+                        this->ps().heading()
+                        + (this->ps().right() ? + M_PI / 2 : - M_PI / 2)
+                )
+                + (this->cc().df() + 0.01) * sin(
+                        this->ps().heading() + M_PI
+                )
+        );
+
+        std::queue<BicycleCar, std::list<BicycleCar>> q;
+        while (!this->collide()) {
+                q.push(this->cc());
+                this->cc().rotate(
+                        this->ps().x4(),
+                        this->ps().y4() - 0.01,
+                        0.01
+                );
         }
         // BFS - find entry current car `cc` and corresponding goal car `gc`
         unsigned int iter_cntr = 0;
-        while (!q.empty() && iter_cntr < 100) {
+        while (!q.empty() && iter_cntr < 30) {
                 this->cc() = BicycleCar(q.front());
                 q.pop();
                 while (
@@ -513,7 +496,7 @@ void PSPlanner::fer_perpendicular()
                 this->cc().sp(-0.01);
         else
                 this->cc().sp(0.01);
-        this->cc().st(this->cc().wb() / this->cc().mtr());
+        this->cc().set_max_steer();
         if (this->ps().right())
                 this->cc().st(this->cc().st() * -1);
         while (!this->left()) {
-- 
2.39.2