Find entry for left parallel, too

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

diff --git a/src/pslot.cc b/src/pslot.cc
index 5f25efd52d703256bf8ad632c3756381e9f674e1..64deb5b39cf5a1f4e43bb8de1dd1b54771558ae1 100644 (file)
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -95,6 +95,54 @@ ParkingSlot::h() const
        return atan2(this->lfy() - this->lry(), this->lfx() - this->lrx());
 }
 
+Point
+ParkingSlot::lf() const
+{
+       return Point(this->lfx(), this->lfy());
+}
+
+Point
+ParkingSlot::lr() const
+{
+       return Point(this->lrx(), this->lry());
+}
+
+Point
+ParkingSlot::rr() const
+{
+       return Point(this->rrx(), this->rry());
+}
+
+Point
+ParkingSlot::rf() const
+{
+       return Point(this->rfx(), this->rfy());
+}
+
+Line
+ParkingSlot::entry() const
+{
+       return this->entry_;
+}
+
+Line
+ParkingSlot::rear() const
+{
+       return this->rear_;
+}
+
+Line
+ParkingSlot::curb() const
+{
+       return this->curb_;
+}
+
+Line
+ParkingSlot::front() const
+{
+       return this->front_;
+}
+
 void
 ParkingSlot::set_parking_speed(double s)
 {
@@ -209,7 +257,6 @@ ParkingSlot::steer_in_slot(BicycleCar c)
                        c.st(c.st() * -1.0);
                }
        }
-       path.push_back(c);
        return path;
 }
 
@@ -217,7 +264,11 @@ PoseRange
 ParkingSlot::fe(BicycleCar c)
 {
        assert(this->parallel());
-       assert(this->right());
+       bool swapped = false;
+       if (!this->right()) {
+               this->swap_side();
+               swapped = true;
+       }
        c.h(this->h());
        double clen = -this->offset_ + this->len() - c.df();
        double cw = c.w() / 2.0;
@@ -251,42 +302,27 @@ ParkingSlot::fe(BicycleCar c)
                }
        }
        if (entries.size() == 0) {
-               return PoseRange();
+               return PoseRange(Pose(0.0, 0.0, 0.0), Pose(0.0, 0.0, 0.0));
        }
        if (entries.size() == 1) {
-               PoseRange pr;
-               pr.x(entries.front().front().x());
-               pr.y(entries.front().front().y());
-               pr.b(entries.front().front().h());
-               pr.e(entries.front().front().h());
-               return pr;
+               auto f = entries.front().front();
+               return PoseRange(f, f);
        }
        auto& c1 = entries.front().front();
        auto& c2 = entries.back().front();
-       double b = std::min(c1.h(), c2.h());
-       double e = std::max(c1.h(), c2.h());
-       clen = c.len();
-       Point b1(c1.x() - clen * cos(c1.h()), c1.y() - clen * sin(c1.h()));
-       Point b2(c2.x() - clen * cos(c2.h()), c2.y() - clen * sin(c2.h()));
-       Point e1(c1.x() + clen * cos(c1.h()), c1.y() + clen * sin(c1.h()));
-       Point e2(c2.x() + clen * cos(c2.h()), c2.y() + clen * sin(c2.h()));
-       Line li1(b1, e1);
-       Line li2(b2, e2);
-       li1.intersects_with(li2);
-       PoseRange pr;
-       pr.x(li1.i1().x());
-       pr.y(li1.i1().y());
-       pr.b(b);
-       pr.e(e);
-       return pr;
+       PoseRange p(c1, c2);
+       if (swapped) {
+               this->swap_side();
+               p.reflect(this->entry_);
+       }
+       return p;
 }
 
 PoseRange
 ParkingSlot::recompute_entry(PoseRange p)
 {
        p.rotate(Point(0.0, 0.0), this->h());
-       p.x(p.x() + this->lrx());
-       p.y(p.y() + this->lry());
+       p.translate(this->border_[0]);
        if (!this->right()) {
                p.reflect(this->entry_);
        }