Rewrite guess gc for forward perpendicular parking

diff --git a/src/psp.cc b/src/psp.cc
index dc81f8401a2ff3af3246bc1757dbec107511417d..449b9a5aa59fa5a58cca83cb72f5ac8bdf4500ab 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -110,17 +110,72 @@ void PSPlanner::guess_gc()
                         - this->ps().heading()
                 ) < M_PI / 2) {
                         // forward parking
-                        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);
+                        this->gc_to_4();
+                        double bx;
+                        double by;
+                        double cx;
+                        double cy;
+                        if (this->ps().right()) {
+                                bx = this->gc().lfx();
+                                by = this->gc().lfy();
+                                cx = this->gc().ccr().x();
+                                cy = this->gc().ccr().y();
+                        } else {
+                                bx = this->gc().rfx();
+                                by = this->gc().rfy();
+                                cx = this->gc().ccl().x();
+                                cy = this->gc().ccl().y();
+                        }
+                        double radi_angl = atan2(by - cy, bx - cx);
+                        radi_angl += dts;
+                        double angl_delta = this->gc().h() - radi_angl;
+                        this->gc().rotate(bx, by, angl_delta);
+                        // TODO there is a bug somewhere :/
+                        //
+                        // cli returns not exact intersection, therefore the
+                        // distance to x1, y1 of border is shorter. Then, when
+                        // moving, the distance `dist_o` is not sufficient and
+                        // car still collide with parking slot. It shouldn't be
+                        // problem until it collides with obstacle.
+                        //
+                        if (this->ps().right()) {
+                                cx = this->gc().ccr().x();
+                                cy = this->gc().ccr().y();
+                        } else {
+                                cx = this->gc().ccl().x();
+                                cy = this->gc().ccl().y();
+                        }
+                        auto cli = circle_line_intersection(
+                                cx, cy, this->gc().iradi(),
+                                this->ps().x1(), this->ps().y1(),
+                                this->ps().x2(), this->ps().y2()
+                        );
+                        double d1 = edist(
+                                this->ps().x1(), this->ps().y1(),
+                                std::get<0>(cli), std::get<1>(cli)
+                        );
+                        double d2 = edist(
+                                this->ps().x1(), this->ps().y1(),
+                                std::get<2>(cli), std::get<3>(cli)
+                        );
+                        double dist_o = std::min<double>(d1, d2);
+                        double angl_o = atan2(
+                                this->ps().y4() - this->ps().y3(),
+                                this->ps().x4() - this->ps().x3()
+                        );
+                        // projection
+                        double angl_d = atan2(
+                                this->ps().y1() - this->ps().y2(),
+                                this->ps().x1() - this->ps().x2()
+                        );
+                        angl_d -= angl_o;
+                        dist_o *= cos(angl_d);
+                        this->gc().x(this->gc().x() + dist_o * cos(angl_o));
+                        this->gc().y(this->gc().y() + dist_o * sin(angl_o));
+                        // --- ENDTODO ---
+                        this->gc().sp(-0.01);
+                        this->gc().st(dts);
+                        return;
                 } else {
                         dts = atan2(
                                 this->ps().y2() - this->ps().y1(),