From 4e16614ec39587cb604e765407b28150f64cf3c2 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Tue, 7 Jul 2020 11:30:29 +0200 Subject: [PATCH] Add decision options for backward --- src/psp.cc | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/psp.cc b/src/psp.cc index 65dddf5..1dac5b6 100644 --- 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) -- 2.39.2