} else {
continue;
}
+ } else if (!this->ps().right() && this->cc().sp() < 0) {
+ double ccrx = this->cc().ccr().x();
+ double ccry = this->cc().ccr().y();
+ double ccr_rr = edist(
+ ccrx, ccry,
+ this->cc().rrx(), this->cc().rry()
+ );
+ double ccr_lr = edist(
+ ccrx, ccry,
+ this->cc().lrx(), this->cc().lry()
+ );
+ double ccr_p1 = edist(
+ ccrx, ccry,
+ this->ps().x1(), this->ps().y1()
+ );
+ if (ccr_lr < ccr_p1) {
+ // pass parking slot
+ continue;
+ } else if (ccr_lr >= ccr_p1 && ccr_rr < ccr_p1) {
+ // partially out of parking slot
+ auto cli1 = ::intersect(
+ ccrx, ccry, ccr_p1,
+ this->cc().lrx(), this->cc().lry(),
+ this->cc().rrx(), this->cc().rry()
+ );
+ double a1 = ::angle_between_closer_point(
+ this->ps().x1(), this->ps().y1(),
+ ccrx, ccry,
+ std::get<1>(cli1), std::get<2>(cli1),
+ std::get<3>(cli1), std::get<4>(cli1)
+ );
+ auto cli2 = ::intersect(
+ ccrx, ccry, ccr_lr,
+ this->ps().x2(), this->ps().y2(),
+ this->ps().x3(), this->ps().y3()
+ );
+ double a2 = angle_between_closer_point(
+ this->cc().lrx(), this->cc().lry(),
+ ccrx, ccry,
+ std::get<1>(cli2), std::get<2>(cli2),
+ std::get<3>(cli2), std::get<4>(cli2)
+ );
+ if (std::get<0>(cli1) && (
+ !std::get<0>(cli2)
+ || a1 < a2
+ )) {
+ this->cc().rotate(ccrx, ccry, a1);
+ if (!::right_side_of_line(
+ this->cc().x(), this->cc().y(),
+
+ this->cc().x()
+ + cos(this->ps().heading()),
+ this->cc().y()
+ + sin(this->ps().heading()),
+
+ this->cc().x()
+ + cos(this->cc().h()),
+ this->cc().y()
+ + sin(this->cc().h())
+ )) {
+ continue;
+ }
+ } else if (std::get<0>(cli2) && (
+ !std::get<0>(cli1)
+ || a2 < a1
+ )) {
+ this->cc().rotate(ccrx, ccry, a2);
+ } else {
+ continue;
+ }
+ } else if (ccr_rr >= ccr_p1) {
+ // in parking slot
+ auto cli1 = ::intersect(
+ ccrx, ccry, ccr_rr,
+ this->ps().x1(), this->ps().y1(),
+ this->ps().x2(), this->ps().y2()
+ );
+ double a1 = angle_between_closer_point(
+ this->cc().rrx(), this->cc().rry(),
+ ccrx, ccry,
+ std::get<1>(cli1), std::get<2>(cli1),
+ std::get<3>(cli1), std::get<4>(cli1)
+ );
+ auto cli2 = ::intersect(
+ ccrx, ccry, ccr_lr,
+ this->ps().x2(), this->ps().y2(),
+ this->ps().x3(), this->ps().y3()
+ );
+ double a2 = angle_between_closer_point(
+ this->cc().lrx(), this->cc().lry(),
+ ccrx, ccry,
+ std::get<1>(cli2), std::get<2>(cli2),
+ std::get<3>(cli2), std::get<4>(cli2)
+ );
+ if (std::get<0>(cli1) && (
+ !std::get<0>(cli2)
+ || a1 < a2
+ )) {
+ this->cc().rotate(ccrx, ccry, a1);
+ if (!::right_side_of_line(
+ this->cc().x(), this->cc().y(),
+
+ this->cc().x()
+ + cos(this->ps().heading()),
+ this->cc().y()
+ + sin(this->ps().heading()),
+
+ this->cc().x()
+ + cos(this->cc().h()),
+ this->cc().y()
+ + sin(this->cc().h())
+ )) {
+ continue;
+ }
+ } else if (std::get<0>(cli2) && (
+ !std::get<0>(cli1)
+ || a2 < a1
+ )) {
+ this->cc().rotate(ccrx, ccry, a2);
+ } else {
+ continue;
+ }
+ }
} else {
// TODO left parking slot (both forward, backward)
}