assert(this->parallel());
assert(this->right());
c.h(this->h());
- double clen = this->offset_ + this->len() - c.df();
- double cw = this->offset_ + c.w() / 2.0;
+ 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();
- c.sp(-0.01);
- auto const& b3 = this->border_[3];
- this->curb_.intersects_with(b3, c.len());
+ c.sp(-0.001);
+ auto const rc = c.rf();
+ this->curb_.intersects_with(rc, c.len());
double max_to_slot;
auto const& rr = c.rr();
auto const& i1 = this->curb_.i1();
auto const& i2 = this->curb_.i2();
if (rr.edist(i1) < rr.edist(i2)) {
- max_to_slot = rr.min_angle_between(b3, i1);
+ max_to_slot = rr.min_angle_between(rc, i1);
} else {
- max_to_slot = rr.min_angle_between(b3, i2);
+ max_to_slot = rr.min_angle_between(rc, 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);
+ a_to_slot += 0.0001;
+ c.rotate(rc, 0.0001);
starts.push_back(c);
}
std::vector<std::vector<BicycleCar>> entries;
if (entries.size() == 0) {
return PoseRange();
}
+ 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& c1 = entries.front().front();
auto& c2 = entries.back().front();
double b = std::min(c1.h(), c2.h());