Refactor left method

Use lf, rf, rr, and rf instead of car center cc.

diff --git a/src/psp.cc b/src/psp.cc
index f42ae01037c93c8c6181811abd669ee65b06dd8d..2f35edc16c7eb4a2480153561742d2c968ef426e 100644 (file)
--- 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