Remove duplicated code

diff --git a/src/psp.cc b/src/psp.cc
index 99f5d3d71d1c1f0e460ce6fd287e291891cfee41..0c79883b4aad07d390560c625549d324a2248ce3 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -556,128 +556,6 @@ void PSPlanner::fe_parallel()
                                 std::get<1>(cli3), std::get<2>(cli3),
                                 std::get<3>(cli3), std::get<4>(cli3)
                         );
-{
-        double rf = sqrt(
-                pow(this->cc().lfy() - ccry, 2)
-                + pow(this->cc().lfx() - ccrx, 2)
-        );
-        auto clif = ::intersect(
-                ccrx, ccry, rf,
-                this->ps().x1(), this->ps().y1(),
-                this->ps().x4(), this->ps().y4()
-        );
-        if (std::get<0>(clif)) {
-                double xf = std::get<1>(clif);
-                double 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);
-                }
-                auto 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);
-                if (
-                        !this->collide()
-                        && (edist(
-                                this->ps().x1(), this->ps().y1(),
-                                xf, yf
-                        ) < edist(
-                                this->ps().x1(), this->ps().y1(),
-                                this->ps().x4(), this->ps().y4()
-                        ))
-                ) {
-                        this->cc().sp(-0.01);
-                        this->cc().set_max_steer();
-                        this->cc().st(this->cc().st() * -1);
-                        this->gc() = BicycleCar(this->cc());
-                        goto successfinish;
-                } else {
-                        this->cc() = BicycleCar(tmp_cc);
-                }
-        } else {
-                // should be parked and found in previous iteration or continue
-                // with the parking process
-        }
-}
-                        double r1 = sqrt(
-                            pow(this->cc().rfy() - ccry, 2)
-                            + pow(this->cc().rfx() - ccrx, 2)
-                        );
-                        auto cli1 = ::intersect(
-                            ccrx, ccry, r1,
-                            this->ps().x3(), this->ps().y3(),
-                            this->ps().x4(), this->ps().y4()
-                        );
-                        double a11 = ::angle_between_three_points(
-                            this->cc().lrx(), this->cc().lry(),
-                            ccrx, ccry,
-                            std::get<1>(cli1), std::get<2>(cli1)
-                        );
-                        double a12 = ::angle_between_three_points(
-                            this->cc().lrx(), this->cc().lry(),
-                            ccrx, ccry,
-                            std::get<3>(cli1), std::get<4>(cli1)
-                        );
-                        double a1 = std::min(a11, a12);
-
-                        double r2 = sqrt(
-                            pow(this->cc().lfy() - ccry, 2)
-                            + pow(this->cc().lfx() - ccrx, 2)
-                        );
-                        auto cli2 = ::intersect(
-                            ccrx, ccry, r2,
-                            this->ps().x3(), this->ps().y3(),
-                            this->ps().x4(), this->ps().y4()
-                        );
-                        double a21 = ::angle_between_three_points(
-                            this->cc().rrx(), this->cc().rry(),
-                            ccrx, ccry,
-                            std::get<1>(cli2), std::get<2>(cli2)
-                        );
-                        double a22 = ::angle_between_three_points(
-                            this->cc().rrx(), this->cc().rry(),
-                            ccrx, ccry,
-                            std::get<3>(cli2), std::get<4>(cli2)
-                        );
-                        double a2 = std::min(a21, a22);
-
-                        double r3 = sqrt(
-                            pow(this->cc().rfy() - ccry, 2)
-                            + pow(this->cc().rfx() - ccrx, 2)
-                        );
-                        auto cli3 = ::intersect(
-                            ccrx, ccry, r3,
-                            this->ps().x3(), this->ps().y3(),
-                            this->ps().x2(), this->ps().y2()
-                        );
-                        double a31 = ::angle_between_three_points(
-                            this->cc().rrx(), this->cc().rry(),
-                            ccrx, ccry,
-                            std::get<1>(cli3), std::get<2>(cli3)
-                        );
-                        double a32 = ::angle_between_three_points(
-                            this->cc().rrx(), this->cc().rry(),
-                            ccrx, ccry,
-                            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))