} else if (ccl_rr >= ccl_p1 && ccl_lr < ccl_p1) {
// partially out of parking slot
// TODO (p1, p2) x (lr, rr)
- // TODO (ccl, rr) x (p2, p3)
+ auto cli2 = ::intersect(
+ cclx, ccly, ccl_rr,
+ this->ps().x2(), this->ps().y2(),
+ this->ps().x3(), this->ps().y3()
+ );
+ double a21 = ::angle_between_three_points(
+ this->cc().rrx(), this->cc().rry(),
+ cclx, ccly,
+ std::get<1>(cli2), std::get<2>(cli2)
+ );
+ double a22 = ::angle_between_three_points(
+ this->cc().rrx(), this->cc().rry(),
+ cclx, ccly,
+ std::get<3>(cli2), std::get<4>(cli2)
+ );
+ double a2 = std::min(a21, a22);
} else if (ccl_lr >= ccl_p1) {
// in parking slot
// TODO (ccl, lr) x (p1, p2)
- // TODO (ccl, rr) x (p2, p3)
+ auto cli2 = ::intersect(
+ cclx, ccly, ccl_rr,
+ this->ps().x2(), this->ps().y2(),
+ this->ps().x3(), this->ps().y3()
+ );
+ double a21 = ::angle_between_three_points(
+ this->cc().rrx(), this->cc().rry(),
+ cclx, ccly,
+ std::get<1>(cli2), std::get<2>(cli2)
+ );
+ double a22 = ::angle_between_three_points(
+ this->cc().rrx(), this->cc().rry(),
+ cclx, ccly,
+ std::get<3>(cli2), std::get<4>(cli2)
+ );
+ double a2 = std::min(a21, a22);
}
double r1 = sqrt(