Add decision options for backward

diff --git a/src/psp.cc b/src/psp.cc
index 65dddf569f8c4907619477f24eaba61322e61d97..1dac5b6e7eba1b7a46607fbc51b5181e9296ebc5 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -349,6 +349,30 @@ void PSPlanner::fe_parallel()
                 if (this->ps().right() && this->cc().sp() < 0) {
                         double cclx = this->cc().ccl().x();
                         double ccly = this->cc().ccl().y();
+                        double ccl_lr = edist(
+                                cclx, ccly,
+                                this->cc().lrx(), this->cc().lry()
+                        );
+                        double ccl_rr = edist(
+                                cclx, ccly,
+                                this->cc().rrx(), this->cc().rry()
+                        );
+                        double ccl_p1 = edist(
+                                cclx, ccly,
+                                this->ps().x1(), this->ps().y1()
+                        );
+                        if (ccl_rr < ccl_p1) {
+                                // pass parking slot
+                                continue;
+                        } else if (ccl_rr >= ccl_p1 && ccl_lr < ccl_p1) {
+                                // partially out of parking slot
+                                // TODO (p1, p2) x (lr, rr)
+                                // TODO (ccl, rr) x (p2, p3)
+                        } else if (ccl_lr >= ccl_p1) {
+                                // in parking slot
+                                // TODO (ccl, lr) x (p1, p2)
+                                // TODO (ccl, rr) x (p2, p3)
+                        }
 
                         double r1 = sqrt(
                             pow(this->cc().lry() - ccly, 2)