From bef1d83b02a6c74cdceb17d1def0ef01a047afcb Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Mon, 22 Jul 2019 13:34:32 +0200 Subject: [PATCH] Add left method implementation --- src/psp.cc | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/psp.cc b/src/psp.cc index 0b84195..fe7d2f5 100644 --- a/src/psp.cc +++ b/src/psp.cc @@ -89,6 +89,29 @@ bool PSPlanner::collide() return false; } +bool PSPlanner::left() +{ + double lfx = this->cc().lfx(); + double lfy = this->cc().lfy(); + 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 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; +} + // find entry void PSPlanner::fe() { -- 2.39.2