/// <summary>Constructors</summary>
public m4x4(v4 x, v4 y, v4 z, v4 w) : this()
{
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
/// <summary>Create a rotation from an axis and angle</summary>
public static m3x4 Rotation(v4 axis_norm, v4 axis_sine_angle, float cos_angle)
{
Debug.Assert(Math_.FEql(axis_norm.LengthSq, 1f), "'axis_norm' should be normalised");
var mat = new m3x4();
v4 trace_vec = axis_norm * (1.0f - cos_angle);
mat.x.x = trace_vec.x * axis_norm.x + cos_angle;
mat.y.y = trace_vec.y * axis_norm.y + cos_angle;
mat.z.z = trace_vec.z * axis_norm.z + cos_angle;
trace_vec.x *= axis_norm.y;
trace_vec.z *= axis_norm.x;
trace_vec.y *= axis_norm.z;
mat.x.y = trace_vec.x + axis_sine_angle.z;
mat.x.z = trace_vec.z - axis_sine_angle.y;
mat.x.w = 0.0f;
mat.y.x = trace_vec.x - axis_sine_angle.z;
mat.y.z = trace_vec.y + axis_sine_angle.x;
mat.y.w = 0.0f;
mat.z.x = trace_vec.z + axis_sine_angle.y;
mat.z.y = trace_vec.y - axis_sine_angle.x;
mat.z.w = 0.0f;
return(mat);
}
/// <summary>Returns true if 'point' is within this bounding box (within 'tol'erance)</summary>
public bool IsWithin(v4 point, float tol)
{
return
(Math.Abs(point.x - Centre.x) <= Radius.x + tol &&
Math.Abs(point.y - Centre.y) <= Radius.y + tol &&
Math.Abs(point.z - Centre.z) <= Radius.z + tol);
}
/// <summary>Returns a bbox that encloses 'lhs' and 'point'</summary>
public static BBox Union(BBox bbox, v4 point)
{
var bb = bbox;
for (int i = 0; i != 3; ++i)
{
if (bb.Radius[i] < 0.0f)
{
bb.Centre[i] = point[i];
bb.Radius[i] = 0.0f;
}
else
{
var signed_dist = point[i] - bb.Centre[i];
var length = Math.Abs(signed_dist);
if (length > bb.Radius[i])
{
float new_radius = (length + bb.Radius[i]) / 2.0f;
bb.Centre[i] += signed_dist * (new_radius - bb.Radius[i]) / length;
bb.Radius[i] = new_radius;
}
}
}
return(bb);
}
/// <summary>Get/Set components by index</summary>
public v4 this[int i]
{
get
{
switch (i)
{
case 0: return(x);
case 1: return(y);
case 2: return(z);
}
throw new ArgumentException("index out of range", "i");
}
set
{
switch (i)
{
case 0: x = value; return;
case 1: y = value; return;
case 2: z = value; return;
}
throw new ArgumentException("index out of range", "i");
}
}
/// <summary>Returns the squared distance from 'point' to 'bbox'</summary>
public static float DistanceSq(v4 point, BBox bbox)
{
float dist_sq = 0.0f;
v4 lower = bbox.Lower();
v4 upper = bbox.Upper();
if (point.x < lower.x)
{
dist_sq += Math_.Sqr(lower.x - point.x);
}
else if (point.x > upper.x)
{
dist_sq += Math_.Sqr(point.x - upper.x);
}
if (point.y < lower.y)
{
dist_sq += Math_.Sqr(lower.y - point.y);
}
else if (point.y > upper.y)
{
dist_sq += Math_.Sqr(point.y - upper.y);
}
if (point.z < lower.z)
{
dist_sq += Math_.Sqr(lower.z - point.z);
}
else if (point.z > upper.z)
{
dist_sq += Math_.Sqr(point.z - upper.z);
}
return(dist_sq);
}
/// <summary>Return the cross product matrix for 'vec'</summary>
public static m3x4 CPM(v4 vec)
{
return(new m3x4(
new v4(0f, +vec.z, -vec.y, 0f),
new v4(-vec.z, 0f, +vec.x, 0f),
new v4(+vec.y, -vec.x, 0f, 0f)));
}
/// <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>Create from an angular displacement vector. length = angle(rad), direction = axis</summary>
public static m3x4 Rotation(v4 angular_displacement)
{
Debug.Assert(angular_displacement.w == 0, "'angular_displacement' should be a scaled direction vector");
var len = angular_displacement.Length;
return(len > Math_.TinyF
? Rotation(angular_displacement / len, len)
: Identity);
}
/// <summary>Return the acceleration along the spline at 'time'</summary>
public v4 Acceleration(float time)
{
var ddblend = new v4( // the 2nd derivative of blend
6.0f * (1.0f - time),
6.0f * (3.0f * time - 2.0f),
6.0f * (1.0f - 3.0f * time),
6.0f * time);
return((m * ddblend).w0);
}
/// <summary>
/// Return the tangent along the spline at 'time'
/// Notes about velocity:
/// A spline from (0,0,0) to (1,0,0) with control points at (1/3,0,0) and (2/3,0,0) will
/// have a constant velocity of (1,0,0) over the full length of the spline.</summary>
public v4 Velocity(float time)
{
var dblend = new v4( // the derivative of blend
3.0f * (time - 1.0f) * (1.0f - time),
3.0f * (1.0f - time) * (1.0f - 3.0f * time),
3.0f * time * (2.0f - 3.0f * time),
3.0f * time * time);
return((m * dblend).w0);
}
/// <summary>Return the position along the spline at 'time'</summary>
public v4 Position(float time)
{
var blend = new v4(
(1.0f - time) * (1.0f - time) * (1.0f - time),
3.0f * time * (1.0f - time) * (1.0f - time),
3.0f * time * time * (1.0f - time),
time * time * time);
return((m * blend).w1);
}
/// <summary>Create a quaternion from an axis and an angle</summary>
public quat(v4 axis, float angle)
: this()
{
var s = (float)Math.Sin(0.5 * angle);
x = s * axis.x;
y = s * axis.y;
z = s * axis.z;
w = (float)Math.Cos(0.5 * angle);
}
/// <summary>
/// Clip a line segment to a bounding box returning the parametric values of the intersection.
/// Returns false if the line does not intersect with the bounding box.
/// Assumes initial values of 't0' and 't1' have been set.</summary>
public static bool Clip(BBox bbox, v4 point_, v4 direction_, ref float t0, ref float t1)
{
// Convert v4's to float arrays for efficient indexing
var lower = (float[])bbox.Lower();
var upper = (float[])bbox.Upper();
var point = (float[])point_;
var direction = (float[])direction_;
// For all three slabs
for (int i = 0; i != 3; ++i)
{
if (Math.Abs(direction[i]) < Math_.TinyF)
{
// Ray is parallel to slab. No hit if origin not within slab
if (point[i] < lower[i] || point[i] > upper[i])
{
return(false);
}
}
else
{
// Compute intersection t value of ray with near and far plane of slab
float u0 = (lower[i] - point[i]) / direction[i];
float u1 = (upper[i] - point[i]) / direction[i];
// Make u0 be intersection with near plane, u1 with far plane
if (u0 > u1)
{
Math_.Swap(ref u0, ref u1);
}
// Record the tightest bounds on the line segment
if (u0 > t0)
{
t0 = u0;
}
if (u1 < t1)
{
t1 = u1;
}
// Exit with no collision as soon as slab intersection becomes empty
if (t0 > t1)
{
return(false);
}
}
}
// Ray intersects all 3 slabs
return(true);
}
// Orientation matrix to "look" at a point
public static m4x4 LookAt(v4 eye, v4 at, v4 up)
{
Debug.Assert(eye.w == 1.0f && at.w == 1.0f && up.w == 0.0f, "Invalid position/direction vectors passed to LookAt");
Debug.Assert(eye - at != v4.Zero, "LookAt 'eye' and 'at' positions are coincident");
Debug.Assert(!Math_.Parallel(eye - at, up), "LookAt 'forward' and 'up' axes are aligned");
var mat = new m4x4 {
};
mat.z = Math_.Normalise(eye - at);
mat.x = Math_.Normalise(Math_.Cross(up, mat.z));
mat.y = Math_.Cross(mat.z, mat.x);
mat.pos = eye;
return(mat);
}
/// <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>
/// Make an orientation matrix from a direction. Note the rotation around the direction
/// vector is not defined. 'axis' is the axis that 'direction' will become.</summary>
public static m3x4 OriFromDir(AxisId axis, v4 dir, v4?up = null)
{
// Get the preferred up direction (handling parallel cases)
var up_ = up == null || Parallel(up.Value, dir) ? Perpendicular(dir) : up.Value;
var ori = m3x4.Identity;
ori.z = Normalise(Sign(axis) * dir);
ori.x = Normalise(Cross(up_, ori.z));
ori.y = Cross(ori.z, ori.x);
// Permute the column vectors so +Z becomes 'axis'
return(PermuteRotation(ori, Math.Abs(axis)));
}
/// <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>
/// This overload attempts to find the nearest point robustly
/// by testing 3 starting points and returning minimum.</summary>
public static float ClosestPoint(Spline spline, v4 pt, bool bound01 = true)
{
float t0 = ClosestPoint(spline, pt, 0.0f, bound01, 5);
float t1 = ClosestPoint(spline, pt, 0.5f, bound01, 5);
float t2 = ClosestPoint(spline, pt, 1.0f, bound01, 5);
float d0 = (pt - spline.Position(t0)).LengthSq;
float d1 = (pt - spline.Position(t1)).LengthSq;
float d2 = (pt - spline.Position(t2)).LengthSq;
if (d0 < d1 && d0 < d2)
{
return(t0);
}
if (d1 < d0 && d1 < d2)
{
return(t1);
}
return(t2);
}
/// <summary>Create a quadratic from 3 points</summary>
public static Quadratic FromPoints(v2 a, v2 b, v2 c)
{
//' Aa.x2 + Ba.x + C = a.y
//' Ab.x2 + Bb.x + C = a.y
//' Ac.x2 + Bc.x + C = a.y
//' => Ax = y
//' A = |a.x² a.x 1| x = |A| y = |a.y|
//' |b.x² b.x 1| |B| |b.y|
//' |c.x² c.x 1| |C| |c.y|
var M = Math_.Transpose(new m3x4(
new v4(a.x * a.x, a.x, 1, 0),
new v4(b.x * b.x, b.x, 1, 0),
new v4(c.x * c.x, c.x, 1, 0)));
var y = new v4(a.y, b.y, c.y, 0);
var x = Math_.Invert(M) * y;
return(new Quadratic(x.x, x.y, x.z));
}
// Create a cubic from 4 points
public static Cubic FromPoints(v2 a, v2 b, v2 c, v2 d)
{
//' Aa.x³ + Ba.x² + Ca.x + D = a.y
//' Ab.x³ + Bb.x² + Cb.x + D = a.y
//' Ac.x³ + Bc.x² + Cc.x + D = a.y
//' Ad.x³ + Bd.x² + Cd.x + D = a.y
//' => Ax = y
//' A = |a.x² a.x 1| x = |A| y = |a.y|
//' |b.x² b.x 1| |B| |b.y|
//' |c.x² c.x 1| |C| |c.y|
var M = Math_.Transpose(new m4x4(
new v4(a.x * a.x * a.x, a.x * a.x, a.x, 1),
new v4(b.x * b.x * b.x, b.x * b.x, b.x, 1),
new v4(c.x * c.x * c.x, c.x * c.x, c.x, 1),
new v4(d.x * d.x * d.x, d.x * d.x, d.x, 1)));
var y = new v4(a.y, b.y, c.y, d.y);
var x = Math_.Invert(M) * y;
return(new Cubic(x.x, x.y, x.z, x.w));
}
/// <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));
}
public static v4 DegreesToRadians(v4 degrees)
{
return(new v4(DegreesToRadians(degrees.x), DegreesToRadians(degrees.y), DegreesToRadians(degrees.z), DegreesToRadians(degrees.w)));
}
public quat(v4 vec)
: this()
{
this.xyzw = vec;
}
/// <summary>Reinterpret a vector as a quaternion</summary>
public static quat From(v4 vec)
{
return(new quat(vec.x, vec.y, vec.z, vec.w));
}
/// <summary>Create a random 3D rotation matrix</summary>
public static m3x4 Random(v4 axis, float min_angle, float max_angle, Random r)
{
return(Rotation(axis, r.Float(min_angle, max_angle)));
}
public m3x4(v4 x, v4 y, v4 z) : this()
{
this.x = x;
this.y = y;
this.z = z;
}
/// <summary>Create from an axis and angle. 'axis' should be normalised</summary>
public static m3x4 Rotation(v4 axis_norm, float angle)
{
Debug.Assert(Math_.FEql(axis_norm.LengthSq, 1f), "'axis_norm' should be normalised");
return(Rotation(axis_norm, axis_norm * (float)Math.Sin(angle), (float)Math.Cos(angle)));
}
/// <summary>
/// Return the cross product matrix for 'vec'. This matrix can be used to take the cross
/// product of another vector: e.g. Cross(v1, v2) == CrossProductMatrix4x4(v1) * v2</summary>
public static m4x4 CPM(v4 vec, v4 pos)
{
return(new m4x4(CPM(vec), pos));
}
public static v4 RadiansToDegrees(v4 degrees)
{
return(new v4(RadiansToDegrees(degrees.x), RadiansToDegrees(degrees.y), RadiansToDegrees(degrees.z), RadiansToDegrees(degrees.w)));
}