Refactor can park forward

diff --git a/src/psp.cc b/src/psp.cc
index 23217588faf20cf10226122c3edf05484f876fc6..12d2d113eee9e1105ed58f47ab1c51699a91ca0a 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -493,60 +493,16 @@ void PSPlanner::fe_parallel()
                                 this->cc().rfx(), this->cc().rfy()
                         );
                         {
-                                auto clif = ::intersect(
-                                        ccrx, ccry, ccr_lf,
-                                        this->ps().x1(), this->ps().y1(),
-                                        this->ps().x4(), this->ps().y4()
-                                );
                                 double af = std::abs(
                                         this->ps().heading()
                                         - this->cc().h()
                                 );
-                                double xf = this->ps().x4();
-                                double yf = this->ps().y4();
-                                if (std::get<0>(clif)) {
-                                        xf = std::get<1>(clif);
-                                        yf = std::get<2>(clif);
-                                        if (
-                                                edist(
-                                                        this->ps().x4(),
-                                                        this->ps().y4(),
-                                                        std::get<3>(clif),
-                                                        std::get<4>(clif)
-                                                ) < edist(
-                                                        this->ps().x4(),
-                                                        this->ps().y4(),
-                                                        xf, yf
-                                                )
-                                        ) {
-                                                xf = std::get<3>(clif);
-                                                yf = std::get<4>(clif);
-                                        }
-                                        af = ::angle_between_three_points(
-                                                this->cc().lfx(),
-                                                this->cc().lfy(),
-                                                ccrx, ccry,
-                                                xf, yf
-                                        );
-                                }
                                 auto tmp_cc = BicycleCar(this->cc());
                                 this->cc().rotate(ccrx, ccry, -af);
+                                this->gc() = BicycleCar(this->cc());
                                 if (
                                         !this->collide()
                                         && this->parked()
-                                        && (
-                                                edist(
-                                                        this->ps().x1(),
-                                                        this->ps().y1(),
-                                                        xf, yf
-                                                ) < edist(
-                                                        this->ps().x1(),
-                                                        this->ps().y1(),
-                                                        this->ps().x4(),
-                                                        this->ps().y4()
-                                                )
-                                                || !std::get<0>(clif)
-                                        )
                                 ) {
                                         this->cc().sp(this->cc().sp() * -1);
                                         this->gc() = BicycleCar(this->cc());