Add max cusp getter

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

diff --git a/src/pslot.cc b/src/pslot.cc
index 6966377bf8999107a36f3b24645fbdeacccf6b71..46a12a637cf0ca70d64307b8dd66bb3a24748536 100644 (file)
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -1,3 +1,9 @@
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
 #include <cassert>
 #include <cmath>
 #include "pslot.hh"
@@ -95,12 +101,66 @@ 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)
 {
        this->parking_speed_ = s;
 }
 
+unsigned int
+ParkingSlot::get_max_cusp() const
+{
+       return this->max_cusp_;
+}
+
 void
 ParkingSlot::set_max_cusp(unsigned int m)
 {
@@ -181,7 +241,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();
@@ -196,6 +261,40 @@ ParkingSlot::drive_in_slot(BicycleCar c)
        return std::vector<BicycleCar>();
 }
 
+std::vector<BicycleCar>
+ParkingSlot::drive_of_slot(BicycleCar c)
+{
+       assert(this->parallel());
+       assert(this->right());
+       assert(c.len() < this->len());
+       assert(c.w() < this->w());
+       assert(this->parked(c));
+       std::vector<BicycleCar> path;
+       path.reserve(this->max_cusp_ + 2);
+       path.push_back(c);
+       unsigned int cusp = 0;
+       auto b_len = sizeof(this->border_) / sizeof(this->border_[0]);
+       std::vector<Point> b(this->border_, this->border_ + b_len);
+       while (cusp < this->max_cusp_ + 1) {
+               if (!c.lf().inside_of(b) && !c.rf().inside_of(b)) {
+                       if (cusp < this->max_cusp_) {
+                               this->max_cusp_ = cusp;
+                       }
+                       path.push_back(c);
+                       return path;
+               }
+               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;
+               }
+       }
+       return std::vector<BicycleCar>();
+}
+
 std::vector<Pose>
 ParkingSlot::steer_in_slot(BicycleCar c)
 {
@@ -209,21 +308,56 @@ ParkingSlot::steer_in_slot(BicycleCar c)
                        c.st(c.st() * -1.0);
                }
        }
-       path.push_back(c);
        return path;
 }
 
 PoseRange
 ParkingSlot::fe(BicycleCar c)
 {
-       assert(this->parallel());
-       assert(this->right());
+       if (!this->parallel()) {
+               double gd = 0.0;
+               double dd = 0.0;
+               double radi = 0.0;
+               if (this->parking_speed_ < 0) {
+                       gd = c.df();
+                       c.h(this->rear_.h() + M_PI);
+                       c.sp(1.0);
+                       radi = c.iradi();
+               } else {
+                       gd = c.dr();
+                       c.h(this->rear_.h());
+                       c.sp(-1.0);
+                       radi = c.ofradi();
+               }
+               c.x(this->entry_.m().x() + gd * cos(this->rear_.h()));
+               c.y(this->entry_.m().y() + gd * sin(this->rear_.h()));
+               Point cc(0.0, 0.0);
+               if (this->right()) {
+                       cc = c.ccl();
+               } else {
+                       cc = c.ccr();
+               }
+               this->rear_.intersects_with(cc, radi);
+               dd = std::min(this->border_[0].edist(this->rear_.i1()),
+                       this->border_[0].edist(this->rear_.i2()));
+               c.st(0.0);
+               c.sp(c.sp() * dd);
+               c.next();
+               c.sp(this->parking_speed_);
+               return PoseRange(c.x(), c.y(), c.h(), c.h());
+       }
+       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;
        c.x(this->lrx() + clen * cos(c.h()) + cw * cos(c.h() + M_PI / 2.0));
        c.y(this->lry() + clen * sin(c.h()) + cw * sin(c.h() + M_PI / 2.0));
        c.set_max_steer();
+       assert(this->parking_speed_ < 0.0);
        c.sp(this->parking_speed_);
        auto const rc = c.rf();
        this->curb_.intersects_with(rc, c.len());
@@ -259,15 +393,19 @@ ParkingSlot::fe(BicycleCar c)
        }
        auto& c1 = entries.front().front();
        auto& c2 = entries.back().front();
-       return PoseRange(c1, c2);
+       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_);
        }