this->ps().x2() - this->ps().x1()
);
while (h < 0) h += 2 * M_PI;
- x += 2 * cos(h);
- y += 2 * sin(h);
//// This is for backward parking only.
//double entry_width = edist(
pi.push_back(BicycleCar(this->cc()));
}
this->cc() = BicycleCar(orig_cc);
+ } else {
+ if (!this->ps().right()) {
+ this->cc().set_max_steer();
+ for (unsigned int i = 0; i < cnt; i++) {
+ this->cc().next();
+ pi.push_back(BicycleCar(this->cc()));
+ }
+ } else {
+ this->cc().set_max_steer();
+ this->cc().st(this->cc().st() * -1);
+ for (unsigned int i = 0; i < cnt; i++) {
+ this->cc().next();
+ pi.push_back(BicycleCar(this->cc()));
+ }
+ }
+ this->cc() = BicycleCar(orig_cc);
}
return pi;
}