/// <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);
}
public static m3x4 operator *(m3x4 lhs, m3x4 rhs)
{
Math_.Transpose(ref lhs);
return(new m3x4(
new v4(Math_.Dot(lhs.x.xyz, rhs.x.xyz), Math_.Dot(lhs.y.xyz, rhs.x.xyz), Math_.Dot(lhs.z.xyz, rhs.x.xyz), 0f),
new v4(Math_.Dot(lhs.x.xyz, rhs.y.xyz), Math_.Dot(lhs.y.xyz, rhs.y.xyz), Math_.Dot(lhs.z.xyz, rhs.y.xyz), 0f),
new v4(Math_.Dot(lhs.x.xyz, rhs.z.xyz), Math_.Dot(lhs.y.xyz, rhs.z.xyz), Math_.Dot(lhs.z.xyz, rhs.z.xyz), 0f)));
}
public static v3 operator *(m3x4 lhs, v3 rhs)
{
Math_.Transpose(ref lhs);
return(new v3(
Math_.Dot(lhs.x.xyz, rhs),
Math_.Dot(lhs.y.xyz, rhs),
Math_.Dot(lhs.z.xyz, rhs)));
}
public static v2 operator *(m2x2 lhs, v2 rhs)
{
v2 ans;
Math_.Transpose(ref lhs);
ans.x = Math_.Dot(lhs.x, rhs);
ans.y = Math_.Dot(lhs.y, rhs);
return(ans);
}
public static v4 operator *(m3x4 lhs, v4 rhs)
{
Math_.Transpose(ref lhs);
return(new v4(
Math_.Dot(lhs.x, rhs),
Math_.Dot(lhs.y, rhs),
Math_.Dot(lhs.z, rhs),
rhs.w));
}
public static m4x4 operator *(m4x4 lhs, m4x4 rhs)
{
Math_.Transpose(ref lhs);
return(new m4x4(
new v4(Math_.Dot(lhs.x, rhs.x), Math_.Dot(lhs.y, rhs.x), Math_.Dot(lhs.z, rhs.x), Math_.Dot(lhs.w, rhs.x)),
new v4(Math_.Dot(lhs.x, rhs.y), Math_.Dot(lhs.y, rhs.y), Math_.Dot(lhs.z, rhs.y), Math_.Dot(lhs.w, rhs.y)),
new v4(Math_.Dot(lhs.x, rhs.z), Math_.Dot(lhs.y, rhs.z), Math_.Dot(lhs.z, rhs.z), Math_.Dot(lhs.w, rhs.z)),
new v4(Math_.Dot(lhs.x, rhs.w), Math_.Dot(lhs.y, rhs.w), Math_.Dot(lhs.z, rhs.w), Math_.Dot(lhs.w, rhs.w))));
}
public static m2x2 operator *(m2x2 lhs, m2x2 rhs)
{
m2x2 ans;
Math_.Transpose(ref lhs);
ans.x.x = Math_.Dot(lhs.x, rhs.x);
ans.x.y = Math_.Dot(lhs.y, rhs.x);
ans.y.x = Math_.Dot(lhs.x, rhs.y);
ans.y.y = Math_.Dot(lhs.y, rhs.y);
return(ans);
}
public static BBox operator *(m4x4 m, BBox rhs)
{
Debug.Assert(rhs.IsValid, "Transforming an invalid bounding box");
var bb = new BBox(m.pos, v4.Zero);
var mat = Math_.Transpose3x3(m);
bb.Centre.x += Math_.Dot(mat.x, rhs.Centre);
bb.Radius.x += Math_.Dot(Math_.Abs(mat.x), rhs.Radius);
bb.Centre.y += Math_.Dot(mat.y, rhs.Centre);
bb.Radius.y += Math_.Dot(Math_.Abs(mat.y), rhs.Radius);
bb.Centre.z += Math_.Dot(mat.z, rhs.Centre);
bb.Radius.z += Math_.Dot(Math_.Abs(mat.z), rhs.Radius);
return(bb);
}
/// <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);
}
}
/// <summary>Construct a quaternion representing a rotation from 'from' to 'to'</summary>
public quat(v4 from, v4 to)
: this()
{
var d = Math_.Dot(from.xyz, to.xyz);
var s = (float)Math.Sqrt(from.xyz.LengthSq * to.xyz.LengthSq) + d;
var axis = Math_.Cross(from, to);
// vectors are 180 degrees apart
if (Math_.FEql(s, 0))
{
axis = Math_.Perpendicular(to);
s = 0.0f;
}
xyzw = Math_.Normalise(new v4(axis.x, axis.y, axis.z, s));
}
/// <summary>Finds the closest point to the 2d line segment a->b returning the parametric value</summary>
public static float ClosestPoint(v2 a, v2 b, v2 pt)
{
v2 ab = b - a;
// Project 'pt' onto 'ab', but defer divide by 'ab.Length2Sq'
float t = Math_.Dot(pt - a, ab);
if (t <= 0.0f)
{
return(0.0f); // 'point' projects outside 'line', clamp to 0.0f
}
float denom = ab.LengthSq;
if (t >= denom)
{
return(1.0f); // 'point' projects outside 'line', clamp to 1.0f
}
return(t / denom); // 'point' projects inside 'line', do deferred divide now
}
/// <summary>Create a transform representing the rotation from one vector to another. 'from' and 'to' do not have to be normalised</summary>
public static m3x4 Rotation(v4 from, v4 to)
{
Debug.Assert(!Math_.FEql(from.Length, 0));
Debug.Assert(!Math_.FEql(to.Length, 0));
var len = from.Length * to.Length;
// Find the cosine of the angle between the vectors
var cos_angle = Math_.Dot(from, to) / len;
if (cos_angle >= 1f - Math_.TinyF)
{
return(Identity);
}
if (cos_angle <= Math_.TinyF - 1f)
{
return(Rotation(Math_.Normalise(Math_.Perpendicular(from - to)), (float)Math_.TauBy2));
}
// Axis multiplied by sine of the angle
var axis_sine_angle = Math_.Cross(from, to) / len;
var axis_norm = Math_.Normalise(axis_sine_angle);
return(Rotation(axis_norm, axis_sine_angle, cos_angle));
}