FIX 2877ad5

diff --git a/src/psp.cc b/src/psp.cc
index 1e61784d73dbd937bd4b12f986280c9d7111d0ce..348960691829d2c5234f4fc4bd30f9033c444168 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -344,99 +344,100 @@ void PSPlanner::fe_parallel()
         // BFS - find entry current car `cc` and corresponding goal car `gc`
         unsigned int iter_cntr = 0;
         while (!q.empty() && iter_cntr < 30) {
+                std::cerr << "backw " << iter_cntr << std::endl;
                 this->cc() = BicycleCar(q.front());
                 q.pop();
                 if (this->ps().right() && this->cc().sp() < 0) {
-                    double cclx = this->cc().ccl().x();
-                    double ccly = this->cc().ccl().y();
+                        double cclx = this->cc().ccl().x();
+                        double ccly = this->cc().ccl().y();
 
-                    double r1 = sqrt(
-                        pow(this->cc().lry() - ccly, 2)
-                        + pow(this->cc().lrx() - cclx, 2)
-                    );
-                    auto cli1 = ::intersect(
-                        cclx, ccly, r1,
-                        this->ps().x1(), this->ps().y1(),
-                        this->ps().x2(), this->ps().y2()
-                    );
-                    double a11 = ::angle_between_three_points(
-                        this->cc().lrx(), this->cc().lry(),
-                        cclx, ccly,
-                        std::get<1>(cli1), std::get<2>(cli1)
-                    );
-                    double a12 = ::angle_between_three_points(
-                        this->cc().lrx(), this->cc().lry(),
-                        cclx, ccly,
-                        std::get<3>(cli1), std::get<4>(cli1)
-                    );
-                    double a1 = std::min(a11, a12);
+                        double r1 = sqrt(
+                            pow(this->cc().lry() - ccly, 2)
+                            + pow(this->cc().lrx() - cclx, 2)
+                        );
+                        auto cli1 = ::intersect(
+                            cclx, ccly, r1,
+                            this->ps().x1(), this->ps().y1(),
+                            this->ps().x2(), this->ps().y2()
+                        );
+                        double a11 = ::angle_between_three_points(
+                            this->cc().lrx(), this->cc().lry(),
+                            cclx, ccly,
+                            std::get<1>(cli1), std::get<2>(cli1)
+                        );
+                        double a12 = ::angle_between_three_points(
+                            this->cc().lrx(), this->cc().lry(),
+                            cclx, ccly,
+                            std::get<3>(cli1), std::get<4>(cli1)
+                        );
+                        double a1 = std::min(a11, a12);
 
-                    double r2 = sqrt(
-                        pow(this->cc().rry() - ccly, 2)
-                        + pow(this->cc().rrx() - cclx, 2)
-                    );
-                    auto cli2 = ::intersect(
-                        cclx, ccly, r2,
-                        this->ps().x1(), this->ps().y1(),
-                        this->ps().x2(), this->ps().y2()
-                    );
-                    double a21 = ::angle_between_three_points(
-                        this->cc().rrx(), this->cc().rry(),
-                        cclx, ccly,
-                        std::get<1>(cli2), std::get<2>(cli2)
-                    );
-                    double a22 = ::angle_between_three_points(
-                        this->cc().rrx(), this->cc().rry(),
-                        cclx, ccly,
-                        std::get<3>(cli2), std::get<4>(cli2)
-                    );
-                    double a2 = std::min(a21, a22);
+                        double r2 = sqrt(
+                            pow(this->cc().rry() - ccly, 2)
+                            + pow(this->cc().rrx() - cclx, 2)
+                        );
+                        auto cli2 = ::intersect(
+                            cclx, ccly, r2,
+                            this->ps().x1(), this->ps().y1(),
+                            this->ps().x2(), this->ps().y2()
+                        );
+                        double a21 = ::angle_between_three_points(
+                            this->cc().rrx(), this->cc().rry(),
+                            cclx, ccly,
+                            std::get<1>(cli2), std::get<2>(cli2)
+                        );
+                        double a22 = ::angle_between_three_points(
+                            this->cc().rrx(), this->cc().rry(),
+                            cclx, ccly,
+                            std::get<3>(cli2), std::get<4>(cli2)
+                        );
+                        double a2 = std::min(a21, a22);
 
-                    double r3 = sqrt(
-                        pow(this->cc().rry() - ccly, 2)
-                        + pow(this->cc().rrx() - cclx, 2)
-                    );
-                    auto cli3 = ::intersect(
-                        cclx, ccly, r3,
-                        this->ps().x2(), this->ps().y2(),
-                        this->ps().x3(), this->ps().y3()
-                    );
-                    double a31 = ::angle_between_three_points(
-                        this->cc().rrx(), this->cc().rry(),
-                        cclx, ccly,
-                        std::get<1>(cli3), std::get<2>(cli3)
-                    );
-                    double a32 = ::angle_between_three_points(
-                        this->cc().rrx(), this->cc().rry(),
-                        cclx, ccly,
-                        std::get<3>(cli3), std::get<4>(cli3)
-                    );
-                    double a3 = std::min(a31, a32);
+                        double r3 = sqrt(
+                            pow(this->cc().rry() - ccly, 2)
+                            + pow(this->cc().rrx() - cclx, 2)
+                        );
+                        auto cli3 = ::intersect(
+                            cclx, ccly, r3,
+                            this->ps().x2(), this->ps().y2(),
+                            this->ps().x3(), this->ps().y3()
+                        );
+                        double a31 = ::angle_between_three_points(
+                            this->cc().rrx(), this->cc().rry(),
+                            cclx, ccly,
+                            std::get<1>(cli3), std::get<2>(cli3)
+                        );
+                        double a32 = ::angle_between_three_points(
+                            this->cc().rrx(), this->cc().rry(),
+                            cclx, ccly,
+                            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(cclx, ccly, -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(cclx, ccly, -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(cclx, ccly, -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(cclx, ccly, -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(cclx, ccly, -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(cclx, ccly, -a3);
+                        } else {
+                            continue;
+                        }
                 } else if (this->ps().right() && this->cc().sp() > 0) {
                         std::cerr << "forw " << iter_cntr << std::endl;
                         double ccrx = this->cc().ccr().x();