Add compute entries ut, impl

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

diff --git a/src/pslot.cc b/src/pslot.cc
index 1d84e28ab1593aa5f5a84e21b91d3e97784dbd07..d6593f2008ae2438f36aa55733231f0dcafe033d 100644 (file)
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -222,6 +222,19 @@ ParkingSlot::collide(BicycleCar const& c) const
                || c.front().intersects_with(this->_front);
 }
 
+void
+ParkingSlot::set_to_start(BicycleCar& c)
+{
+       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);
+}
+
 std::vector<BicycleCar>
 ParkingSlot::drive_in_slot(BicycleCar c)
 {
@@ -351,14 +364,7 @@ ParkingSlot::fe(BicycleCar c)
                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);
+       this->set_to_start(c);
        auto const rc = c.rf();
        this->_curb.intersects_with(rc, c.len());
        double max_to_slot;
@@ -380,18 +386,18 @@ ParkingSlot::fe(BicycleCar c)
        for (auto s: starts) {
                auto r = this->drive_in_slot(s);
                if (r.size() > 0) {
-                       this->_entries.push_back(r);
+                       this->_ispaths.push_back(r);
                }
        }
-       if (this->_entries.size() == 0) {
+       if (this->_ispaths.size() == 0) {
                return PoseRange(Pose(0.0, 0.0, 0.0), Pose(0.0, 0.0, 0.0));
        }
-       if (this->_entries.size() == 1) {
-               auto f = this->_entries.front().front();
+       if (this->_ispaths.size() == 1) {
+               auto f = this->_ispaths.front().front();
                return PoseRange(f, f);
        }
-       auto& c1 = this->_entries.front().front();
-       auto& c2 = this->_entries.back().front();
+       auto& c1 = this->_ispaths.front().front();
+       auto& c2 = this->_ispaths.back().front();
        PoseRange p(c1, c2);
        if (swapped) {
                this->swap_side();
@@ -400,6 +406,23 @@ ParkingSlot::fe(BicycleCar c)
        return p;
 }
 
+void
+ParkingSlot::compute_entries()
+{
+       assert(this->_ispaths.size() > 0);
+       assert(this->right());
+       this->_entries.clear();
+       for (auto p: this->_ispaths) {
+               this->_entries.push_back(p.front());
+       }
+       for (auto& e: this->_entries) {
+               e.sp(e.sp() * -1.0);
+               while (!this->lf().inside_of(e.ccr(), e.iradi())) {
+                       e.next();
+               }
+       }
+}
+
 PoseRange
 ParkingSlot::recompute_entry(PoseRange p)
 {
@@ -411,6 +434,14 @@ ParkingSlot::recompute_entry(PoseRange p)
        return p;
 }
 
+void
+ParkingSlot::gen_gnuplot_to(std::ostream& out)
+{
+       this->rear().gen_gnuplot_to(out);
+       this->curb().gen_gnuplot_to(out);
+       this->front().gen_gnuplot_to(out);
+}
+
 std::ostream&
 operator<<(std::ostream& o, ParkingSlot const& s)
 {