From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 11 Mar 2019 06:25:59 +0000 (+0100)
Subject: Update slot side getter
X-Git-Tag: v0.6.0~14^2~9
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/8b60df5742b5be98f44ce27c8a23759fd38e8193

Update slot side getter
---

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 81c4b7d..b0131fd 100644
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -60,9 +60,7 @@ SlotSide ParallelSlot::slotSide()
                 float x1 = this->slot().bnodes()[1]->x();
                 float y3 = this->slot().bnodes()[3]->y();
                 float x3 = this->slot().bnodes()[3]->x();
-                float h1 = atan2(y0 - y3, x0 - x3);
-                float h2 = atan2(y1 - y3, x1 - x3);
-                if (h2 - h1 > 0)
+                if (sgn((x1 - x3) * (y0 - y3) - (y1 - y3) * (x0 - x3)) < 0)
                         this->slotSide_ = LEFT;
                 else
                         this->slotSide_ = RIGHT;