Add todos for forward

diff --git a/src/psp.cc b/src/psp.cc
index 34d4b7c8afa96f25384c1250a08775497dbde2ad..3776cf7879eca848337b5c264426a21c017d3f20 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -453,6 +453,18 @@ void PSPlanner::fe_parallel()
                 } else if (this->ps().right() && this->cc().sp() > 0) {
                         double ccrx = this->cc().ccr().x();
                         double ccry = this->cc().ccr().y();
+                        double ccr_lf = edist(
+                                ccrx, ccry,
+                                this->cc().lfx(), this->cc().lfy()
+                        );
+                        double ccr_rf = edist(
+                                ccrx, ccry,
+                                this->cc().rfx(), this->cc().rfy()
+                        );
+                        // TODO check if I can park
+                        // 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)