continue;
}
}
+ } else if (!this->ps().right() && this->cc().sp() > 0) {
+ double cclx = this->cc().ccl().x();
+ double ccly = this->cc().ccl().y();
+ double ccl_rf = edist(
+ cclx, ccly,
+ this->cc().rfx(), this->cc().rfy()
+ );
+ double ccl_lf = edist(
+ cclx, ccly,
+ this->cc().lfx(), this->cc().lfy()
+ );
+ {
+ double af = std::abs(
+ this->ps().heading()
+ - this->cc().h()
+ );
+ auto tmp_cc = BicycleCar(this->cc());
+ this->cc().rotate(cclx, ccly, af);
+ this->gc() = BicycleCar(this->cc());
+ if (
+ !this->collide()
+ && this->parked()
+ ) {
+ this->cc().sp(this->cc().sp() * -1);
+ this->gc() = BicycleCar(this->cc());
+ goto successfinish;
+ } else {
+ this->cc() = BicycleCar(tmp_cc);
+ }
+ }
+ auto cli1 = ::intersect(
+ cclx, ccly, ccl_lf,
+ this->ps().x3(), this->ps().y3(),
+ this->ps().x4(), this->ps().y4()
+ );
+ double a1 = angle_between_closer_point(
+ this->cc().lfx(), this->cc().lfy(),
+ cclx, ccly,
+ std::get<1>(cli1), std::get<2>(cli1),
+ std::get<3>(cli1), std::get<4>(cli1)
+ );
+ auto cli2 = ::intersect(
+ cclx, ccly, ccl_lf,
+ this->ps().x2(), this->ps().y2(),
+ this->ps().x3(), this->ps().y3()
+ );
+ double a2 = angle_between_closer_point(
+ this->cc().lfx(), this->cc().lfy(),
+ cclx, ccly,
+ std::get<1>(cli2), std::get<2>(cli2),
+ std::get<3>(cli2), std::get<4>(cli2)
+ );
+ auto cli3 = ::intersect(
+ cclx, ccly, ccl_rf,
+ this->ps().x3(), this->ps().y3(),
+ this->ps().x4(), this->ps().y4()
+ );
+ double a3 = angle_between_closer_point(
+ this->cc().rfx(), this->cc().rfy(),
+ cclx, ccly,
+ std::get<1>(cli3), std::get<2>(cli3),
+ std::get<3>(cli3), std::get<4>(cli3)
+ );
+ if (std::get<0>(cli1) && (
+ (!std::get<0>(cli2) && !std::get<0>(cli3))
+ || (a1 < a2 && !std::get<0>(cli3))
+ || (a1 < a3 && !std::get<0>(cli2))
+ || (a1 < a2 && a1 < a3)
+ )) {
+ this->cc().rotate(cclx, ccly, a1);
+ } else if (std::get<0>(cli2) && (
+ (!std::get<0>(cli1) && !std::get<0>(cli3))
+ || (a2 < a1 && !std::get<0>(cli3))
+ || (a2 < a3 && !std::get<0>(cli1))
+ || (a2 < a1 && a2 < a3)
+ )) {
+ this->cc().rotate(cclx, ccly, a2);
+ } else if (std::get<0>(cli3) && (
+ (!std::get<0>(cli1) && !std::get<0>(cli2))
+ || (a3 < a1 && !std::get<0>(cli2))
+ || (a3 < a2 && !std::get<0>(cli1))
+ || (a3 < a1 && a3 < a2)
+ )) {
+ this->cc().rotate(cclx, ccly, a3);
+ } else {
+ continue;
+ }
} else {
// TODO left parking slot (both forward, backward)
}