Add find entry method

diff --git a/incl/pslot.hh b/incl/pslot.hh
index b970cbacdbe21667fa8fc87fcfc604ed56772920..c669d23e7e81d653bce5f18558bc373819a4aa6d 100644 (file)
--- a/incl/pslot.hh
+++ b/incl/pslot.hh
@@ -16,6 +16,7 @@ namespace bcar {
  */
 class ParkingSlot {
 private:
+       double offset_ = 0.001; // to avoid collision during init
        Point border_[4];
        Line entry_;
        Line rear_;
@@ -88,6 +89,9 @@ public:
         */
        std::vector<BicycleCar> drive_in_slot(BicycleCar c, unsigned int& max);
 
+       /*! Find entry. */
+       PoseRange fe();
+
        friend std::ostream& operator<<(std::ostream& o, ParkingSlot const& s);
 };
 

diff --git a/src/pslot.cc b/src/pslot.cc
index aff8850ded9ab3245b1da4607ee2813593b3f505..07b2cdbeb95d0984ead2dcd26592db4e5546192e 100644 (file)
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -174,6 +174,66 @@ ParkingSlot::drive_in_slot(BicycleCar c, unsigned int& max)
        return path;
 }
 
+PoseRange
+ParkingSlot::fe()
+{
+       assert(this->parallel());
+       assert(this->right());
+       BicycleCar c;
+       c.h(this->h());
+       double clen = this->offset_ + this->len() - c.df();
+       double cw = this->offset_ + 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();
+       c.sp(-0.01);
+       auto const& b3 = this->border_[3];
+       this->curb_.intersects_with(b3, c.len());
+       double max_to_slot;
+       auto const& rr = c.rr();
+       auto const& i1 = this->curb_.in1();
+       auto const& i2 = this->curb_.in2();
+       if (rr.edist(i1) < rr.edist(i2)) {
+               max_to_slot = rr.min_angle_between(b3, i1);
+       } else {
+               max_to_slot = rr.min_angle_between(b3, i2);
+       }
+       std::vector<BicycleCar> starts;
+       double a_to_slot = 0.0;
+       while (a_to_slot < max_to_slot) {
+               a_to_slot += 0.001;
+               c.rotate(b3, 0.001);
+               starts.push_back(c);
+       }
+       std::vector<std::vector<BicycleCar>> entries;
+       unsigned int max_cusp = 10;
+       for (auto s: starts) {
+               auto r = this->drive_in_slot(s, max_cusp);
+               if (r.size() > 0) {
+                       entries.push_back(r);
+               }
+       }
+       assert(entries.size() > 0);
+       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.in1().x());
+       pr.y(li1.in1().y());
+       pr.b(b);
+       pr.e(e);
+       return pr;
+}
+
 std::ostream&
 operator<<(std::ostream& o, ParkingSlot const& s)
 {