/// <summary>
/// Find the closest point on 'spline' to 'pt'
/// Note: the analytic solution to this problem involves solving a 5th order polynomial
/// This method uses Newton's method and relies on a "good" initial estimate of the nearest point
/// Should have quadratic convergence</summary>
public static float ClosestPoint(Spline spline, v4 pt, float initial_estimate, bool bound01 = true, int iterations = 5)
{
// The distance (sqr'd) from 'pt' to the spline is: Dist(t) = |pt - S(t)|^2. (S(t) = spline at t)
// At the closest point, Dist'(t) = 0.
// Dist'(t) = -2(pt - S(t)).S'(t)
// So we want to find 't' such that Dist'(t) = 0
// Newton's method of iteration = t_next = t_current - f(x)/f'(x)
// f(x) = Dist'(t)
// f'(x) = Dist''(t) = 2S'(t).S'(t) - 2(pt - S(t)).S''(t)
float time = initial_estimate;
for (int iter = 0; iter != iterations; ++iter)
{
v4 S = spline.Position(time);
v4 dS = spline.Velocity(time);
v4 ddS = spline.Acceleration(time);
v4 R = pt - S;
time += Math_.Dot(R, dS) / (Math_.Dot(dS, dS) - Math_.Dot(R, ddS));
if (bound01 && time <= 0.0f || time >= 1.0f)
{
return(Math_.Clamp(time, 0.0f, 1.0f));
}
}
return(time);
}
/// <summary>Return the closest point on 'rect' to 'pt'</summary>
public static v2 ClosestPoint(BRect rect, v2 pt)
{
v2 lower = rect.Lower;
v2 upper = rect.Upper;
v2 closest;
if (rect.IsWithin(pt))
{
// if pt.x/pt.y > rect.sizeX/rect.sizeY then the point
// is closer to the Y edge of the rectangle
if (Math_.Abs(pt.x * rect.SizeY) > Math_.Abs(pt.y * rect.SizeX))
{
closest = new v2(pt.x, Math_.Sign(pt.y) * rect.SizeY);
}
else
{
closest = new v2(Math_.Sign(pt.x) * rect.SizeX, pt.y);
}
}
else
{
closest = new v2(
Math_.Clamp(pt.x, lower.x, upper.x),
Math_.Clamp(pt.y, lower.y, upper.y));
}
return(closest);
}
/// <summary>Return the closest point between two line segments</summary>
public static void ClosestPoint(v4 s0, v4 e0, v4 s1, v4 e1, out float t0, out float t1)
{
Debug.Assert(s0.w == 1f && e0.w == 1f && s1.w == 1f && e1.w == 1f);
v4 line0 = e0 - s0;
v4 line1 = e1 - s1;
v4 separation = s0 - s1;
float f = Math_.Dot(line1, separation);
float c = Math_.Dot(line0, separation);
float line0_length_sq = line0.LengthSq;
float line1_length_sq = line1.LengthSq;
// Check if either or both segments are degenerate
if (Math_.FEql(line0_length_sq, 0f) && Math_.FEql(line1_length_sq, 0f))
{
t0 = 0.0f; t1 = 0.0f; return;
}
if (Math_.FEql(line0_length_sq, 0f))
{
t0 = 0.0f; t1 = Math_.Clamp(f / line1_length_sq, 0.0f, 1.0f); return;
}
if (Math_.FEql(line1_length_sq, 0f))
{
t1 = 0.0f; t0 = Math_.Clamp(-c / line0_length_sq, 0.0f, 1.0f); return;
}
// The general nondegenerate case starts here
float b = Math_.Dot(line0, line1);
float denom = line0_length_sq * line1_length_sq - b * b; // Always non-negative
// If segments not parallel, calculate closest point on infinite line 'line0'
// to infinite line 'line1', and clamp to segment 1. Otherwise pick arbitrary t0
t0 = denom != 0.0f ? Math_.Clamp((b * f - c * line1_length_sq) / denom, 0.0f, 1.0f) : 0.0f;
// Calculate point on infinite line 'line1' closest to segment 'line0' at t0
// using t1 = Dot3(pt0 - s1, line1) / line1_length_sq = (b*t0 + f) / line1_length_sq
t1 = (b * t0 + f) / line1_length_sq;
// If t1 in [0,1] then done. Otherwise, clamp t1, recompute t0 for the new value
// of t1 using t0 = Dot3(pt1 - s0, line0) / line0_length_sq = (b*t1 - c) / line0_length_sq
// and clamped t0 to [0, 1]
if (t1 < 0.0f)
{
t1 = 0.0f; t0 = Math_.Clamp((-c) / line0_length_sq, 0.0f, 1.0f);
}
else if (t1 > 1.0f)
{
t1 = 1.0f; t0 = Math_.Clamp((b - c) / line0_length_sq, 0.0f, 1.0f);
}
}