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)
{