Add drive in slot method

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

diff --git a/src/pslot.cc b/src/pslot.cc
index 2246b5253e27e410156bac649d1319fb11badcaa..aff8850ded9ab3245b1da4607ee2813593b3f505 100644 (file)
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -114,6 +114,66 @@ ParkingSlot::swap_side()
        this->border_[2].rotate(this->border_[3], M_PI);
 }
 
+bool
+ParkingSlot::parked(BicycleCar const& c) const
+{
+       auto b_len = sizeof(this->border_) / sizeof(this->border_[0]);
+       std::vector<Point> b(this->border_, this->border_ + b_len);
+       return c.lf().inside_of(b) && c.lr().inside_of(b)
+               && c.rr().inside_of(b) && c.rf().inside_of(b);
+}
+
+bool
+ParkingSlot::collide(BicycleCar const& c) const
+{
+       return c.left().intersects_with(this->rear_)
+               && c.left().intersects_with(this->curb_)
+               && c.left().intersects_with(this->front_)
+               && c.rear().intersects_with(this->rear_)
+               && c.rear().intersects_with(this->curb_)
+               && c.rear().intersects_with(this->front_)
+               && c.right().intersects_with(this->rear_)
+               && c.right().intersects_with(this->curb_)
+               && c.right().intersects_with(this->front_)
+               && c.front().intersects_with(this->rear_)
+               && c.front().intersects_with(this->curb_)
+               && c.front().intersects_with(this->front_);
+}
+
+std::vector<BicycleCar>
+ParkingSlot::drive_in_slot(BicycleCar c, unsigned int& max)
+{
+       assert(this->parallel());
+       assert(this->right());
+       assert(c.len() < this->len());
+       assert(c.w() < this->w());
+       std::vector<BicycleCar> path;
+       path.reserve(max + 2);
+       path.push_back(c);
+       unsigned int cusp = 0;
+       while (cusp < max) {
+               if (c.h() < this->h()) {
+                       return std::vector<BicycleCar>();
+               }
+               if (this->parked(c)) {
+                       if (cusp < max) {
+                               max = cusp;
+                       }
+                       break;
+               }
+               c.next();
+               if (this->collide(c)) {
+                       c.sp(c.sp() * -1.0);
+                       c.next();
+                       path.push_back(c);
+                       c.st(c.st() * -1.0);
+                       cusp += 1;
+               }
+       }
+       path.push_back(c);
+       return path;
+}
+
 std::ostream&
 operator<<(std::ostream& o, ParkingSlot const& s)
 {