From 652ab671c5d27d0049844c4bbf35ae4d6b944c2a Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Fri, 3 Jul 2020 17:27:18 +0200 Subject: [PATCH] Solve find intersection todo --- src/psp.cc | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/psp.cc b/src/psp.cc index c120b05..7f1f2b7 100644 --- a/src/psp.cc +++ b/src/psp.cc @@ -376,7 +376,24 @@ void PSPlanner::fe_parallel() 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 + auto clif = ::intersect( + cclx, ccly, rf, + this->cc().lrx(), this->cc().lry(), + this->cc().rrx(), this->cc().rry() + ); + double xf = std::get<1>(clif); + double yf = std::get<2>(clif); + if ( + edist( + std::get<3>(clif), std::get<4>(clif), + this->cc().x(), this->cc().y() + + ) + < edist(xf, yf, this->cc().x(), this->cc().y()) + ) { + xf = std::get<3>(clif); + yf = std::get<4>(clif); + } // TODO find angle: intersection, ccl, p1 // TODO rotate by such angle } -- 2.39.2