Stop backward when car heading < slot heading

diff --git a/src/psp.cc b/src/psp.cc
index 0c79883b4aad07d390560c625549d324a2248ce3..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