60 start_pos.
y = -start_pos.
y;
62 curvature = -curvature;
66 if (curvature < 0.0f) {
68 start_pos.
y = -start_pos.
y;
70 curvature = -curvature;
78 const float radius = 1.0f / curvature;
80 const Vector3D circ_center(0.0f, radius, 0.0f);
85 if (rotated_p == circ_center) {
86 return std::make_pair(0.0f, radius);
91 const Vector3D intersection = ((rotated_p - circ_center).MakeUnitVector() * radius) + circ_center;
95 const float last_point_angle = length / radius;
97 constexpr float pi_half = Pi<float>() / 2.0f;
102 float angle = std::atan2(intersection.
y - radius, intersection.
x) + pi_half;
106 angle += Pi<float>() * 2.0f;
112 if (angle <= last_point_angle) {
113 return std::make_pair(
124 radius * std::cos(last_point_angle - pi_half),
125 radius * std::sin(last_point_angle - pi_half) + circ_center.
y,
128 const float end_dist =
Distance2D(end_pos, rotated_p);
131 return (start_dist < end_dist)?
132 std::make_pair(0.0f, start_dist) :
133 std::make_pair(length, end_dist);
static std::pair< float, float > DistanceArcToPoint(Vector3D p, Vector3D start_pos, float length, float heading, float curvature)
计算点到弧的距离 返回一个包含: