FIX 17f58f0

diff --git a/src/psp.cc b/src/psp.cc
index 70fb6b7aaa50196ddc7b063e9c53363e109c133b..1e61784d73dbd937bd4b12f986280c9d7111d0ce 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -438,96 +438,97 @@ void PSPlanner::fe_parallel()
                         continue;
                     }
                 } else if (this->ps().right() && this->cc().sp() > 0) {
-                    double ccrx = this->cc().ccr().x();
-                    double ccry = this->cc().ccr().y();
+                        std::cerr << "forw " << iter_cntr << std::endl;
+                        double ccrx = this->cc().ccr().x();
+                        double ccry = this->cc().ccr().y();
 
-                    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 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 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);
+                        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))
-                        || (a1 < a3 && !std::get<0>(cli2))
-                        || (a1 < a2 && a1 < a3)
-                    )) {
-                        this->cc().rotate(ccrx, ccry, -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(ccrx, ccry, -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(ccrx, ccry, -a3);
-                    } else {
-                        continue;
-                    }
+                        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(ccrx, ccry, -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(ccrx, ccry, -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(ccrx, ccry, -a3);
+                        } else {
+                            continue;
+                        }
                 } else {
                         // TODO left parking slot (both forward, backward)
                 }