Implement right parking slot bacward movement

diff --git a/src/psp.cc b/src/psp.cc
index 31d4a741dd17b23f877ca52a33874e164d1dbc86..c440944484031d768682b2bed67dd67dc0870ad6 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -346,6 +346,102 @@ void PSPlanner::fe_parallel()
         while (!q.empty() && iter_cntr < 30) {
                 this->cc() = BicycleCar(q.front());
                 q.pop();
+                if (this->ps().right() && this->cc().sp() < 0) {
+                    double cclx = this->cc().ccl().x();
+                    double ccly = this->cc().ccl().y();
+
+                    double r1 = sqrt(
+                        pow(this->cc().lry() - ccly, 2)
+                        + pow(this->cc().lrx() - cclx, 2)
+                    );
+                    auto cli1 = ::intersect(
+                        cclx, ccly, r1,
+                        this->ps().x1(), this->ps().y1(),
+                        this->ps().x2(), this->ps().y2()
+                    );
+                    double a11 = ::angle_between_three_points(
+                        this->cc().lrx(), this->cc().lry(),
+                        cclx, ccly,
+                        std::get<1>(cli1), std::get<2>(cli1)
+                    );
+                    double a12 = ::angle_between_three_points(
+                        this->cc().lrx(), this->cc().lry(),
+                        cclx, ccly,
+                        std::get<3>(cli1), std::get<4>(cli1)
+                    );
+                    double a1 = std::min(a11, a12);
+
+                    double r2 = sqrt(
+                        pow(this->cc().rry() - ccly, 2)
+                        + pow(this->cc().rrx() - cclx, 2)
+                    );
+                    auto cli2 = ::intersect(
+                        cclx, ccly, r2,
+                        this->ps().x1(), this->ps().y1(),
+                        this->ps().x2(), this->ps().y2()
+                    );
+                    double a21 = ::angle_between_three_points(
+                        this->cc().rrx(), this->cc().rry(),
+                        cclx, ccly,
+                        std::get<1>(cli2), std::get<2>(cli2)
+                    );
+                    double a22 = ::angle_between_three_points(
+                        this->cc().rrx(), this->cc().rry(),
+                        cclx, ccly,
+                        std::get<3>(cli2), std::get<4>(cli2)
+                    );
+                    double a2 = std::min(a21, a22);
+
+                    double r3 = sqrt(
+                        pow(this->cc().rry() - ccly, 2)
+                        + pow(this->cc().rrx() - cclx, 2)
+                    );
+                    auto cli3 = ::intersect(
+                        cclx, ccly, r3,
+                        this->ps().x2(), this->ps().y2(),
+                        this->ps().x3(), this->ps().y3()
+                    );
+                    double a31 = ::angle_between_three_points(
+                        this->cc().rrx(), this->cc().rry(),
+                        cclx, ccly,
+                        std::get<1>(cli3), std::get<2>(cli3)
+                    );
+                    double a32 = ::angle_between_three_points(
+                        this->cc().rrx(), this->cc().rry(),
+                        cclx, ccly,
+                        std::get<3>(cli3), std::get<4>(cli3)
+                    );
+                    double a3 = std::min(a31, a32);
+
+                    if (std::get<0>(cli1) && (
+                        (!std::get<0>(cli2) && !std::get<0>(cli3))
+                        || (a1 < a2 && !std::get<0>(cli3))
+                        || (a1 < a3 && !std::get<0>(cli2))
+                        || (a1 < a2 && a1 < a3)
+                    )) {
+                        this->cc().rotate(cclx, ccly, -a1);
+                    } else if (std::get<0>(cli2) && (
+                        (!std::get<0>(cli1) && !std::get<0>(cli3))
+                        || (a2 < a1 && !std::get<0>(cli3))
+                        || (a2 < a3 && !std::get<0>(cli1))
+                        || (a2 < a1 && a2 < a3)
+                    )) {
+                        this->cc().rotate(cclx, ccly, -a2);
+                    } else if (std::get<0>(cli3) && (
+                        (!std::get<0>(cli1) && !std::get<0>(cli2))
+                        || (a3 < a1 && !std::get<0>(cli2))
+                        || (a3 < a2 && !std::get<0>(cli1))
+                        || (a3 < a1 && a3 < a2)
+                    )) {
+                        this->cc().rotate(cclx, ccly, -a3);
+                    } else {
+                        continue;
+                    }
+                } else if (this->ps().right() && this->cc().sp() > 0) {
+                    // TODO right parking slot (forward)
+                } else {
+                    // TODO left parking slot (both forward, backward)
+                }
                 this->cc().sp(this->cc().sp() * -1);
                 this->cc().next();
                 this->gc() = BicycleCar(this->cc());