Add left backward for parallel parking

diff --git a/src/psp.cc b/src/psp.cc
index cc11af7388cb052e0c7205afc826cdc8ef4ffcb1..aaf21bf8d75cdd44967cefc38f39678df77d94e9 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -551,6 +551,129 @@ void PSPlanner::fe_parallel()
                         } else {
                             continue;
                         }
+                } else if (!this->ps().right() && this->cc().sp() < 0) {
+                        double ccrx = this->cc().ccr().x();
+                        double ccry = this->cc().ccr().y();
+                        double ccr_rr = edist(
+                                ccrx, ccry,
+                                this->cc().rrx(), this->cc().rry()
+                        );
+                        double ccr_lr = edist(
+                                ccrx, ccry,
+                                this->cc().lrx(), this->cc().lry()
+                        );
+                        double ccr_p1 = edist(
+                                ccrx, ccry,
+                                this->ps().x1(), this->ps().y1()
+                        );
+                        if (ccr_lr < ccr_p1) {
+                                // pass parking slot
+                                continue;
+                        } else if (ccr_lr >= ccr_p1 && ccr_rr < ccr_p1) {
+                                // partially out of parking slot
+                                auto cli1 = ::intersect(
+                                        ccrx, ccry, ccr_p1,
+                                        this->cc().lrx(), this->cc().lry(),
+                                        this->cc().rrx(), this->cc().rry()
+                                );
+                                double a1 = ::angle_between_closer_point(
+                                        this->ps().x1(), this->ps().y1(),
+                                        ccrx, ccry,
+                                        std::get<1>(cli1), std::get<2>(cli1),
+                                        std::get<3>(cli1), std::get<4>(cli1)
+                                );
+                                auto cli2 = ::intersect(
+                                        ccrx, ccry, ccr_lr,
+                                        this->ps().x2(), this->ps().y2(),
+                                        this->ps().x3(), this->ps().y3()
+                                );
+                                double a2 = angle_between_closer_point(
+                                        this->cc().lrx(), this->cc().lry(),
+                                        ccrx, ccry,
+                                        std::get<1>(cli2), std::get<2>(cli2),
+                                        std::get<3>(cli2), std::get<4>(cli2)
+                                );
+                                if (std::get<0>(cli1) && (
+                                        !std::get<0>(cli2)
+                                        || a1 < a2
+                                )) {
+                                        this->cc().rotate(ccrx, ccry, a1);
+                                        if (!::right_side_of_line(
+                                                this->cc().x(), this->cc().y(),
+
+                                                this->cc().x()
+                                                + cos(this->ps().heading()),
+                                                this->cc().y()
+                                                + sin(this->ps().heading()),
+
+                                                this->cc().x()
+                                                + cos(this->cc().h()),
+                                                this->cc().y()
+                                                + sin(this->cc().h())
+                                        )) {
+                                                continue;
+                                        }
+                                } else if (std::get<0>(cli2) && (
+                                        !std::get<0>(cli1)
+                                        || a2 < a1
+                                )) {
+                                        this->cc().rotate(ccrx, ccry, a2);
+                                } else {
+                                        continue;
+                                }
+                        } else if (ccr_rr >= ccr_p1) {
+                                // in parking slot
+                                auto cli1 = ::intersect(
+                                        ccrx, ccry, ccr_rr,
+                                        this->ps().x1(), this->ps().y1(),
+                                        this->ps().x2(), this->ps().y2()
+                                );
+                                double a1 = angle_between_closer_point(
+                                        this->cc().rrx(), this->cc().rry(),
+                                        ccrx, ccry,
+                                        std::get<1>(cli1), std::get<2>(cli1),
+                                        std::get<3>(cli1), std::get<4>(cli1)
+                                );
+                                auto cli2 = ::intersect(
+                                        ccrx, ccry, ccr_lr,
+                                        this->ps().x2(), this->ps().y2(),
+                                        this->ps().x3(), this->ps().y3()
+                                );
+                                double a2 = angle_between_closer_point(
+                                        this->cc().lrx(), this->cc().lry(),
+                                        ccrx, ccry,
+                                        std::get<1>(cli2), std::get<2>(cli2),
+                                        std::get<3>(cli2), std::get<4>(cli2)
+                                );
+                                if (std::get<0>(cli1) && (
+                                        !std::get<0>(cli2)
+                                        || a1 < a2
+                                )) {
+                                        this->cc().rotate(ccrx, ccry, a1);
+                                        if (!::right_side_of_line(
+                                                this->cc().x(), this->cc().y(),
+
+                                                this->cc().x()
+                                                + cos(this->ps().heading()),
+                                                this->cc().y()
+                                                + sin(this->ps().heading()),
+
+                                                this->cc().x()
+                                                + cos(this->cc().h()),
+                                                this->cc().y()
+                                                + sin(this->cc().h())
+                                        )) {
+                                                continue;
+                                        }
+                                } else if (std::get<0>(cli2) && (
+                                        !std::get<0>(cli1)
+                                        || a2 < a1
+                                )) {
+                                        this->cc().rotate(ccrx, ccry, a2);
+                                } else {
+                                        continue;
+                                }
+                        }
                 } else {
                         // TODO left parking slot (both forward, backward)
                 }