- double r3 = sqrt(
- pow(this->cc().rry() - ccly, 2)
- + pow(this->cc().rrx() - cclx, 2)
- );
- auto cli3 = ::intersect(
- cclx, ccly, r3,
- this->ps().x2(), this->ps().y2(),
- this->ps().x3(), this->ps().y3()
- );
- double a31 = ::angle_between_three_points(
- this->cc().rrx(), this->cc().rry(),
- cclx, ccly,
- std::get<1>(cli3), std::get<2>(cli3)
- );
- double a32 = ::angle_between_three_points(
- this->cc().rrx(), this->cc().rry(),
- cclx, ccly,
- std::get<3>(cli3), std::get<4>(cli3)
- );
- double a3 = std::min(a31, a32);
+ 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(cclx, ccly, -a2);
+ } else {
+ continue;
+ }
+ } else if (ccl_lr >= ccl_p1) {
+ // in parking slot
+ auto cli1 = ::intersect(
+ cclx, ccly, ccl_lr,
+ this->ps().x1(), this->ps().y1(),
+ this->ps().x2(), this->ps().y2()
+ );
+ double a1 = angle_between_closer_point(
+ this->cc().lrx(), this->cc().lry(),
+ cclx, ccly,
+ std::get<1>(cli1), std::get<2>(cli1),
+ std::get<3>(cli1), std::get<4>(cli1)
+ );
+ auto cli2 = ::intersect(
+ cclx, ccly, ccl_rr,
+ this->ps().x2(), this->ps().y2(),
+ this->ps().x3(), this->ps().y3()
+ );
+ double a2 = angle_between_closer_point(
+ this->cc().rrx(), this->cc().rry(),
+ cclx, ccly,
+ 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(cclx, ccly, -a1);
+ if (::right_side_of_line(
+ this->cc().x(), this->cc().y(),