Add fix backward movement todo

diff --git a/src/psp.cc b/src/psp.cc
index 29d220a6a3f59890ffc7038dbca0254d5e04e8f3..c120b056dcc0e9c2e9db39486e4a0734612b9dfc 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -370,6 +370,17 @@ void PSPlanner::fe_parallel()
                             std::get<3>(cli1), std::get<4>(cli1)
                         );
                         double a1 = std::min(a11, a12);
+{
+        double rf = edist(cclx, ccly, this->ps().x1(), this->ps().y1());
+        if (
+                edist(cclx, ccly, this->cc().lrx(), this->cc().lry()) < rf
+                && rf < edist(cclx, ccly, this->cc().rrx(), this->cc().rry())
+        ) {
+                // TODO find intersection lrx, lry -> rf -> rrx, rry
+                // TODO find angle: intersection, ccl, p1
+                // TODO rotate by such angle
+        }
+}
 
                         double r2 = sqrt(
                             pow(this->cc().rry() - ccly, 2)