Decide forward/backward based on 1st & 2nd border

diff --git a/src/psp.cc b/src/psp.cc
index d9635df388eacfc13d49586500c8e2e892b21c18..e4410682baa9ff5f6dbab0570031153947242e4d 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -70,12 +70,14 @@ void PSPlanner::guess_gc()
                 y += (this->gc().w() / 2 + 0.01) * sin(h + dts);
                 y += (this->gc().dr() + 0.01) * sin(h);
         } else {
-                dts = atan2(
-                        this->ps().y2() - this->ps().y1(),
-                        this->ps().x2() - this->ps().x1()
-                );
-                dts *= 1.01; // precision workaround
-                if (std::abs(dts - this->ps().heading()) < M_PI / 2) {
+                if (std::abs(
+                        atan2(
+                                this->ps().y2() - this->ps().y1(),
+                                this->ps().x2() - this->ps().x1()
+                        )
+                        - this->ps().heading()
+                ) < M_PI / 2) {
+                        // forward parking
                         x = this->ps().x4();
                         y = this->ps().y4();
                         h = dts;
@@ -88,6 +90,12 @@ void PSPlanner::guess_gc()
                         x += (this->gc().w() / 2 + 0.01) * cos(dts);
                         y += (this->gc().w() / 2 + 0.01) * sin(dts);
                 } else {
+                        dts = atan2(
+                                this->ps().y2() - this->ps().y1(),
+                                this->ps().x2() - this->ps().x1()
+                        );
+                        dts *= 1.01; // precision workaround
+                        // backward parking
                         h = dts + M_PI;
                         x += -(this->gc().df() + 0.01) * cos(h);
                         y += -(this->gc().df() + 0.01) * sin(h);