Check if should continue by forward move

If the car heading points towards parking slot, it should not continue.
Instead, the next iteration should be considered.

diff --git a/src/psp.cc b/src/psp.cc
index 31c71c3d044854dd7e07629a91f04182041ee7cc..aabe5ec1867f424b6584e86a6b9151fff109194d 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -437,7 +437,14 @@ void PSPlanner::fe_parallel()
                         } else {
                             continue;
                         }
-                        // TODO if cc.h < ps.h --> continue
+                        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 (this->ps().right() && this->cc().sp() > 0) {
                         double ccrx = this->cc().ccr().x();
                         double ccry = this->cc().ccr().y();