Add drive in slot method

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

diff --git a/src/pslot.cc b/src/pslot.cc
index d9f9b21a3ae79f028c36b1967ebf8f2cf654c4dd..aff8850ded9ab3245b1da4607ee2813593b3f505 100644 (file)
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -11,13 +11,36 @@ ParkingSlot::ParkingSlot(Point p, double h, double W, double L) :
                        Point(p.x() + W * cos(h - M_PI / 2.0) + L * cos(h),
                                p.y() + W * sin(h - M_PI / 2.0) + L * sin(h)),
                        Point(p.x() + L * cos(h), p.y() + L * sin(h))}),
-               entry_(p, border_[3]),
-               rear_(p, border_[1]),
+               entry_(border_[0], border_[3]),
+               rear_(border_[0], border_[1]),
                curb_(border_[1], border_[2]),
                front_(border_[2], border_[3])
 {
 }
 
+ParkingSlot::ParkingSlot(double lrx, double lry, double rrx, double rry,
+               double rfx, double rfy, double lfx, double lfy) :
+                       border_({Point(lrx, lry), Point(rrx, rry),
+                               Point(rfx, rfy), Point(lfx, lfy)}),
+                       entry_(border_[0], border_[3]),
+                       rear_(border_[0], border_[1]),
+                       curb_(border_[1], border_[2]),
+                       front_(border_[2], border_[3])
+{
+}
+
+double
+ParkingSlot::len() const
+{
+       return this->entry_.len();
+}
+
+double
+ParkingSlot::w() const
+{
+       return this->rear_.len();
+}
+
 double
 ParkingSlot::lfx() const
 {
@@ -91,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)
 {