From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Wed, 8 Jul 2020 09:29:15 +0000 (+0200)
Subject: Stop backward when car heading < slot heading
X-Git-Tag: v0.4.0~4^2~5
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/commitdiff_plain/3aa179347f1a2ae2ae476193046a1774ab1d0f22

Stop backward when car heading < slot heading
---

diff --git a/src/psp.cc b/src/psp.cc
index 0c79883..3fc91d4 100644
--- 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