Stop backward when car heading < slot heading

[hubacji1/psp.git] / src / psp.cc

diff --git a/src/psp.cc b/src/psp.cc
index cff29ffcbe75b09c193a835091859e83ae33871a..3fc91d4035a5c5e3f38c429db9dda530ce045e91 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -404,6 +404,21 @@ void PSPlanner::fe_parallel()
                                         || a1 < a2
                                 )) {
                                         this->cc().rotate(cclx, ccly, -a1);
+                                        if (::right_side_of_line(
+                                                this->cc().x(), this->cc().y(),
+
+                                                this->cc().x()
+                                                + cos(this->ps().heading()),
+                                                this->cc().y()
+                                                + sin(this->ps().heading()),
+
+                                                this->cc().x()
+                                                + cos(this->cc().h()),
+                                                this->cc().y()
+                                                + sin(this->cc().h())
+                                        )) {
+                                                continue;
+                                        }
                                 } else if (std::get<0>(cli2) && (
                                         !std::get<0>(cli1)
                                         || a2 < a1
@@ -441,6 +456,21 @@ void PSPlanner::fe_parallel()
                                         || a1 < a2
                                 )) {
                                         this->cc().rotate(cclx, ccly, -a1);
+                                        if (::right_side_of_line(
+                                                this->cc().x(), this->cc().y(),
+
+                                                this->cc().x()
+                                                + cos(this->ps().heading()),
+                                                this->cc().y()
+                                                + sin(this->ps().heading()),
+
+                                                this->cc().x()
+                                                + cos(this->cc().h()),
+                                                this->cc().y()
+                                                + sin(this->cc().h())
+                                        )) {
+                                                continue;
+                                        }
                                 } else if (std::get<0>(cli2) && (
                                         !std::get<0>(cli1)
                                         || a2 < a1
@@ -523,131 +553,39 @@ void PSPlanner::fe_parallel()
                                         this->cc() = BicycleCar(tmp_cc);
                                 }
                         }
-                        // TODO (ccr, rf) x (p3, p4)
-                        // TODO (ccr, rf) x (p2, p3)
-                        // TODO (ccr, lf) x (p3, p4)
-{
-        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)
+                                ccrx, ccry, ccr_rf,
+                                this->ps().x3(), this->ps().y3(),
+                                this->ps().x4(), this->ps().y4()
                         );
-                        double a1 = std::min(a11, a12);
-
-                        double r2 = sqrt(
-                            pow(this->cc().lfy() - ccry, 2)
-                            + pow(this->cc().lfx() - ccrx, 2)
+                        double a1 = smaller_angle_of_two(
+                                this->cc().rfx(), this->cc().rfy(),
+                                ccrx, ccry,
+                                std::get<1>(cli1), std::get<2>(cli1),
+                                std::get<3>(cli1), std::get<4>(cli1)
                         );
                         auto cli2 = ::intersect(
-                            ccrx, ccry, r2,
-                            this->ps().x3(), this->ps().y3(),
-                            this->ps().x4(), this->ps().y4()
+                                ccrx, ccry, ccr_rf,
+                                this->ps().x2(), this->ps().y2(),
+                                this->ps().x3(), this->ps().y3()
                         );
-                        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)
+                        double a2 = smaller_angle_of_two(
+                                this->cc().rfx(), this->cc().rfy(),
+                                ccrx, ccry,
+                                std::get<1>(cli2), std::get<2>(cli2),
+                                std::get<3>(cli2), std::get<4>(cli2)
                         );
                         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)
+                                ccrx, ccry, ccr_lf,
+                                this->ps().x3(), this->ps().y3(),
+                                this->ps().x4(), this->ps().y4()
                         );
-                        double a32 = ::angle_between_three_points(
-                            this->cc().rrx(), this->cc().rry(),
-                            ccrx, ccry,
-                            std::get<3>(cli3), std::get<4>(cli3)
+                        double a3 = smaller_angle_of_two(
+                                this->cc().lfx(), this->cc().lfy(),
+                                ccrx, ccry,
+                                std::get<1>(cli3), std::get<2>(cli3),
+                                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))