From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Fri, 3 Jul 2020 15:14:36 +0000 (+0200)
Subject: Add fix backward movement todo
X-Git-Tag: v0.4.0~4^2~24
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/commitdiff_plain/5820e97c0478fdc27bb2a5690d04079d00c50a20

Add fix backward movement todo
---

diff --git a/src/psp.cc b/src/psp.cc
index 29d220a..c120b05 100644
--- 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)