From 546f7189a7db33d768d9b175a5fa2eb26000aba1 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Wed, 8 Jul 2020 13:49:41 +0200
Subject: [PATCH] Refactor can park forward

---
 src/psp.cc | 46 +---------------------------------------------
 1 file changed, 1 insertion(+), 45 deletions(-)

diff --git a/src/psp.cc b/src/psp.cc
index 2321758..12d2d11 100644
--- 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());
-- 
2.39.2