From 9d97fede9e6712735c29be936228790e4c93fab5 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 22 Jul 2019 15:10:33 +0200 Subject: [PATCH] Refactor left method Use lf, rf, rr, and rf instead of car center cc. --- src/psp.cc | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/psp.cc b/src/psp.cc index f42ae01..2f35edc 100644 --- a/src/psp.cc +++ b/src/psp.cc @@ -93,23 +93,29 @@ bool PSPlanner::left() { double lfx = this->cc().lfx(); double lfy = this->cc().lfy(); + double lrx = this->cc().lrx(); + double lry = this->cc().lry(); + double rrx = this->cc().rrx(); + double rry = this->cc().rry(); double rfx = this->cc().rfx(); double rfy = this->cc().rfy(); - double ccx = this->cc().x(); - double ccy = this->cc().y(); double lfs = sgn( (lfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1()) - (lfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1()) ); + double lrs = sgn( + (lrx - this->ps().x1()) * (this->ps().y4() - this->ps().y1()) + - (lry - this->ps().y1()) * (this->ps().x4() - this->ps().x1()) + ); + double rrs = sgn( + (rrx - this->ps().x1()) * (this->ps().y4() - this->ps().y1()) + - (rry - this->ps().y1()) * (this->ps().x4() - this->ps().x1()) + ); double rfs = sgn( (rfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1()) - (rfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1()) ); - double ccs = sgn( - (ccx - this->ps().x1()) * (this->ps().y4() - this->ps().y1()) - - (ccy - this->ps().y1()) * (this->ps().x4() - this->ps().x1()) - ); - return lfs == rfs && lfs != ccs; + return lfs == rfs && (lfs != lrs || lfs != rrs); } // find entry -- 2.39.2