Refactor guess goal for perpendicular

The problem was with angled parking slots. `guess_gc` method now decide
if the parking will be forward or backward.

diff --git a/src/psp.cc b/src/psp.cc
index 894893704085e552ea67bb32e868be3603110645..9529383aa1c3bddaa873ddbef0b340ccaaa8713e 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -41,20 +41,41 @@ void PSPlanner::guess_gc()
         double dts = + M_PI / 2; // direction to slot
         if (this->ps().right())
                 dts = - M_PI / 2;
         double dts = + M_PI / 2; // direction to slot
         if (this->ps().right())
                 dts = - M_PI / 2;

+        dts *= 0.99; // precision workaround

         if (this->ps().parallel()) {
                 x += (this->gc().w() / 2 + 0.01) * cos(h + dts);
                 x += (this->gc().dr() + 0.01) * cos(h);
                 y += (this->gc().w() / 2 + 0.01) * sin(h + dts);
                 y += (this->gc().dr() + 0.01) * sin(h);
         } else {
         if (this->ps().parallel()) {
                 x += (this->gc().w() / 2 + 0.01) * cos(h + dts);
                 x += (this->gc().dr() + 0.01) * cos(h);
                 y += (this->gc().w() / 2 + 0.01) * sin(h + dts);
                 y += (this->gc().dr() + 0.01) * sin(h);
         } else {

-                x += (this->ps().x4() - this->ps().x1()) / 2;
-                x += (this->gc().df() + 0.01) * cos(h + dts);
-                y += (this->ps().y4() - this->ps().y1()) / 2;
-                y += (this->gc().df() + 0.01) * sin(h + dts);
-                if (this->ps().right())
-                        h += M_PI / 2;
-                else
-                        h -= M_PI / 2;
+                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) {
+                        x = this->ps().x4();
+                        y = this->ps().y4();
+                        h = dts;
+                        x += (this->gc().dr() + 0.01) * cos(h);
+                        y += (this->gc().dr() + 0.01) * sin(h);
+                        if (this->ps().right())
+                                dts -= M_PI / 2;
+                        else
+                                dts += M_PI / 2;
+                        x += (this->gc().w() / 2 + 0.01) * cos(dts);
+                        y += (this->gc().w() / 2 + 0.01) * sin(dts);
+                } else {
+                        h = dts + M_PI;
+                        x += -(this->gc().df() + 0.01) * cos(h);
+                        y += -(this->gc().df() + 0.01) * sin(h);
+                        if (this->ps().right())
+                                dts += M_PI / 2;
+                        else
+                                dts -= M_PI / 2;
+                        x += (this->gc().w() / 2 + 0.01) * cos(dts);
+                        y += (this->gc().w() / 2 + 0.01) * sin(dts);
+                }

         }
         while (h > M_PI)
                 h -= 2 * M_PI;
         }
         while (h > M_PI)
                 h -= 2 * M_PI;