Fix drive in slot end condition

[hubacji1/bcar.git] / src / pslot.cc

diff --git a/src/pslot.cc b/src/pslot.cc
index fe6c3773f6b79cf75815159066887e717fc8009d..4753d91dbbb397d0acd54efef604ed6dcdb82fb4 100644 (file)
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -229,7 +229,12 @@ ParkingSlot::drive_in_slot(BicycleCar c)
                        path.push_back(c);
                        return path;
                }
-               if (c.h() < this->h()) {
+               double sx = c.x() + 10.0 * cos(this->h());
+               double sy = c.y() + 10.0 * sin(this->h());
+               double cx = c.x() + 10.0 * cos(c.h());
+               double cy = c.y() + 10.0 * sin(c.h());
+               if (Point(cx, cy).on_right_side_of(
+                               Line(Point(c.x(), c.y()), Point(sx, sy)))) {
                        return std::vector<BicycleCar>();
                }
                c.next();