// Return the inverse of 'mat' using double precision floats
public static m4x4 InvertPrecise(m4x4 mat)
{
var inv = new double[4, 4];
inv[0, 0] = 0.0 + mat.y.y * mat.z.z * mat.w.w - mat.y.y * mat.z.w * mat.w.z - mat.z.y * mat.y.z * mat.w.w + mat.z.y * mat.y.w * mat.w.z + mat.w.y * mat.y.z * mat.z.w - mat.w.y * mat.y.w * mat.z.z;
inv[0, 1] = 0.0 - mat.x.y * mat.z.z * mat.w.w + mat.x.y * mat.z.w * mat.w.z + mat.z.y * mat.x.z * mat.w.w - mat.z.y * mat.x.w * mat.w.z - mat.w.y * mat.x.z * mat.z.w + mat.w.y * mat.x.w * mat.z.z;
inv[0, 2] = 0.0 + mat.x.y * mat.y.z * mat.w.w - mat.x.y * mat.y.w * mat.w.z - mat.y.y * mat.x.z * mat.w.w + mat.y.y * mat.x.w * mat.w.z + mat.w.y * mat.x.z * mat.y.w - mat.w.y * mat.x.w * mat.y.z;
inv[0, 3] = 0.0 - mat.x.y * mat.y.z * mat.z.w + mat.x.y * mat.y.w * mat.z.z + mat.y.y * mat.x.z * mat.z.w - mat.y.y * mat.x.w * mat.z.z - mat.z.y * mat.x.z * mat.y.w + mat.z.y * mat.x.w * mat.y.z;
inv[1, 0] = 0.0 - mat.y.x * mat.z.z * mat.w.w + mat.y.x * mat.z.w * mat.w.z + mat.z.x * mat.y.z * mat.w.w - mat.z.x * mat.y.w * mat.w.z - mat.w.x * mat.y.z * mat.z.w + mat.w.x * mat.y.w * mat.z.z;
inv[1, 1] = 0.0 + mat.x.x * mat.z.z * mat.w.w - mat.x.x * mat.z.w * mat.w.z - mat.z.x * mat.x.z * mat.w.w + mat.z.x * mat.x.w * mat.w.z + mat.w.x * mat.x.z * mat.z.w - mat.w.x * mat.x.w * mat.z.z;
inv[1, 2] = 0.0 - mat.x.x * mat.y.z * mat.w.w + mat.x.x * mat.y.w * mat.w.z + mat.y.x * mat.x.z * mat.w.w - mat.y.x * mat.x.w * mat.w.z - mat.w.x * mat.x.z * mat.y.w + mat.w.x * mat.x.w * mat.y.z;
inv[1, 3] = 0.0 + mat.x.x * mat.y.z * mat.z.w - mat.x.x * mat.y.w * mat.z.z - mat.y.x * mat.x.z * mat.z.w + mat.y.x * mat.x.w * mat.z.z + mat.z.x * mat.x.z * mat.y.w - mat.z.x * mat.x.w * mat.y.z;
inv[2, 0] = 0.0 + mat.y.x * mat.z.y * mat.w.w - mat.y.x * mat.z.w * mat.w.y - mat.z.x * mat.y.y * mat.w.w + mat.z.x * mat.y.w * mat.w.y + mat.w.x * mat.y.y * mat.z.w - mat.w.x * mat.y.w * mat.z.y;
inv[2, 1] = 0.0 - mat.x.x * mat.z.y * mat.w.w + mat.x.x * mat.z.w * mat.w.y + mat.z.x * mat.x.y * mat.w.w - mat.z.x * mat.x.w * mat.w.y - mat.w.x * mat.x.y * mat.z.w + mat.w.x * mat.x.w * mat.z.y;
inv[2, 2] = 0.0 + mat.x.x * mat.y.y * mat.w.w - mat.x.x * mat.y.w * mat.w.y - mat.y.x * mat.x.y * mat.w.w + mat.y.x * mat.x.w * mat.w.y + mat.w.x * mat.x.y * mat.y.w - mat.w.x * mat.x.w * mat.y.y;
inv[2, 3] = 0.0 - mat.x.x * mat.y.y * mat.z.w + mat.x.x * mat.y.w * mat.z.y + mat.y.x * mat.x.y * mat.z.w - mat.y.x * mat.x.w * mat.z.y - mat.z.x * mat.x.y * mat.y.w + mat.z.x * mat.x.w * mat.y.y;
inv[3, 0] = 0.0 - mat.y.x * mat.z.y * mat.w.z + mat.y.x * mat.z.z * mat.w.y + mat.z.x * mat.y.y * mat.w.z - mat.z.x * mat.y.z * mat.w.y - mat.w.x * mat.y.y * mat.z.z + mat.w.x * mat.y.z * mat.z.y;
inv[3, 1] = 0.0 + mat.x.x * mat.z.y * mat.w.z - mat.x.x * mat.z.z * mat.w.y - mat.z.x * mat.x.y * mat.w.z + mat.z.x * mat.x.z * mat.w.y + mat.w.x * mat.x.y * mat.z.z - mat.w.x * mat.x.z * mat.z.y;
inv[3, 2] = 0.0 - mat.x.x * mat.y.y * mat.w.z + mat.x.x * mat.y.z * mat.w.y + mat.y.x * mat.x.y * mat.w.z - mat.y.x * mat.x.z * mat.w.y - mat.w.x * mat.x.y * mat.y.z + mat.w.x * mat.x.z * mat.y.y;
inv[3, 3] = 0.0 + mat.x.x * mat.y.y * mat.z.z - mat.x.x * mat.y.z * mat.z.y - mat.y.x * mat.x.y * mat.z.z + mat.y.x * mat.x.z * mat.z.y + mat.z.x * mat.x.y * mat.y.z - mat.z.x * mat.x.z * mat.y.y;
var det = mat.x.x * inv[0, 0] + mat.x.y * inv[1, 0] + mat.x.z * inv[2, 0] + mat.x.w * inv[3, 0];
Debug.Assert(det != 0, "matrix has no inverse");
var inv_det = 1.0 / det;
return(new m4x4(
new v4((float)(inv[0, 0] * inv_det), (float)(inv[0, 1] * inv_det), (float)(inv[0, 2] * inv_det), (float)(inv[0, 3] * inv_det)),
new v4((float)(inv[1, 0] * inv_det), (float)(inv[1, 1] * inv_det), (float)(inv[1, 2] * inv_det), (float)(inv[1, 3] * inv_det)),
new v4((float)(inv[2, 0] * inv_det), (float)(inv[2, 1] * inv_det), (float)(inv[2, 2] * inv_det), (float)(inv[2, 3] * inv_det)),
new v4((float)(inv[3, 0] * inv_det), (float)(inv[3, 1] * inv_det), (float)(inv[3, 2] * inv_det), (float)(inv[3, 3] * inv_det))));
}
// Return true if 'mat' is an affine transform
public static bool IsAffine(m4x4 mat)
{
return
(mat.x.w == 0.0f &&
mat.y.w == 0.0f &&
mat.z.w == 0.0f &&
mat.w.w == 1.0f);
}
/// <summary>Return true if any components of 'm' are NaN</summary>
public static bool IsNaN(m4x4 m)
{
return
(IsNaN(m.x) ||
IsNaN(m.y) ||
IsNaN(m.z) ||
IsNaN(m.w));
}
/// <summary>Finite test of matrix elements</summary>
public static bool IsFinite(m4x4 m)
{
return
(IsFinite(m.x) &&
IsFinite(m.y) &&
IsFinite(m.z) &&
IsFinite(m.w));
}
/// <summary>Return the minimum element value in 'm'</summary>
public static float MinElement(m4x4 m)
{
return(Min(
MinElement(m.x),
MinElement(m.y),
MinElement(m.z),
MinElement(m.w)));
}
/// <summary>Return the maximum element value in 'm'</summary>
public static float MaxElement(m4x4 m)
{
return(Max(
MaxElement(m.x),
MaxElement(m.y),
MaxElement(m.z),
MaxElement(m.w)));
}
/// <summary>Absolute value of 'x'</summary>
public static m4x4 Abs(m4x4 x)
{
return(new m4x4(
Abs(x.x),
Abs(x.y),
Abs(x.z),
Abs(x.w)));
}
/// <summary>Approximate equal</summary>
public static bool FEqlAbsolute(m4x4 a, m4x4 b, float tol)
{
return
(FEqlAbsolute(a.x, b.x, tol) &&
FEqlAbsolute(a.y, b.y, tol) &&
FEqlAbsolute(a.z, b.z, tol) &&
FEqlAbsolute(a.w, b.w, tol));
}
/// <summary>Transpose the 4x4 matrix</summary>
public static void Transpose(ref m4x4 m)
{
Swap(ref m.x.y, ref m.y.x);
Swap(ref m.x.z, ref m.z.x);
Swap(ref m.x.w, ref m.w.x);
Swap(ref m.y.z, ref m.z.y);
Swap(ref m.y.w, ref m.w.y);
Swap(ref m.z.w, ref m.w.z);
}
/// <summary>Invert 'm' in-place (assuming an orthonormal matrix</summary>
public static void InvertFast(ref m4x4 m)
{
Debug.Assert(IsOrthonormal(m), "Matrix is not orthonormal");
var trans = m.w;
Transpose(ref m.rot);
m.w.x = -(trans.x * m.x.x + trans.y * m.y.x + trans.z * m.z.x);
m.w.y = -(trans.x * m.x.y + trans.y * m.y.y + trans.z * m.z.y);
m.w.z = -(trans.x * m.x.z + trans.y * m.y.z + trans.z * m.z.z);
}
/// <summary>Spherically interpolate between two affine transforms</summary>
public static m4x4 Slerp(m4x4 lhs, m4x4 rhs, double frac)
{
Debug.Assert(IsAffine(lhs));
Debug.Assert(IsAffine(rhs));
var q = Slerp(new quat(lhs.rot), new quat(rhs.rot), frac);
var p = Lerp(lhs.pos, rhs.pos, frac);
return(new m4x4(q, p));
}
/// <summary>Permute the rotation vectors in a matrix by 'n'</summary>
public static m4x4 Permute(m4x4 mat, int n)
{
switch (n % 3)
{
default: return(mat);
case 1: return(new m4x4(mat.y, mat.z, mat.x, mat.w));
case 2: return(new m4x4(mat.z, mat.x, mat.y, mat.w));
}
}
// 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);
}
// Construct a perspective projection matrix using field of view
public static m4x4 ProjectionPerspectiveFOV(float fovY, float aspect, float zn, float zf, bool righthanded)
{
Debug.Assert(Math_.IsFinite(aspect) && aspect > 0, "invalid aspect ratio");
Debug.Assert(Math_.IsFinite(zn) && Math_.IsFinite(zf) && zn > 0 && zf > 0 && (zn - zf) != 0, "invalid near/far planes");
var rh = Math_.SignF(righthanded);
var mat = new m4x4 {
};
mat.y.y = (float)(1 / Math.Tan(fovY / 2));
mat.x.x = mat.y.y / aspect;
mat.z.w = -rh;
mat.z.z = rh * zf / (zn - zf);
mat.w.z = zn * zf / (zn - zf);
return(mat);
}
// Construct a perspective projection matrix
public static m4x4 ProjectionPerspective(float w, float h, float zn, float zf, bool righthanded)
{
Debug.Assert(Math_.IsFinite(w) && Math_.IsFinite(h) && w > 0 && h > 0, "invalid view rect");
Debug.Assert(Math_.IsFinite(zn) && Math_.IsFinite(zf) && zn > 0 && zf > 0 && (zn - zf) != 0, "invalid near/far planes");
var rh = Math_.SignF(righthanded);
var mat = new m4x4 {
};
mat.x.x = 2.0f * zn / w;
mat.y.y = 2.0f * zn / h;
mat.z.w = -rh;
mat.z.z = rh * zf / (zn - zf);
mat.w.z = zn * zf / (zn - zf);
return(mat);
}
// Construct an orthographic projection matrix
public static m4x4 ProjectionOrthographic(float w, float h, float zn, float zf, bool righthanded)
{
Debug.Assert(Math_.IsFinite(w) && Math_.IsFinite(h) && w > 0 && h > 0, "invalid view rect");
Debug.Assert(Math_.IsFinite(zn) && Math_.IsFinite(zf) && (zn - zf) != 0, "invalid near/far planes");
var rh = Math_.SignF(righthanded);
var mat = new m4x4 {
};
mat.x.x = 2.0f / w;
mat.y.y = 2.0f / h;
mat.z.z = rh / (zn - zf);
mat.w.w = 1.0f;
mat.w.z = rh * zn / (zn - zf);
return(mat);
}
/// <summary>Return the 4x4 determinant of the arbitrary transform 'mat'</summary>
public static float Determinant(m4x4 m)
{
var c1 = (m.z.z * m.w.w) - (m.z.w * m.w.z);
var c2 = (m.z.y * m.w.w) - (m.z.w * m.w.y);
var c3 = (m.z.y * m.w.z) - (m.z.z * m.w.y);
var c4 = (m.z.x * m.w.w) - (m.z.w * m.w.x);
var c5 = (m.z.x * m.w.z) - (m.z.z * m.w.x);
var c6 = (m.z.x * m.w.y) - (m.z.y * m.w.x);
return
(m.x.x * (m.y.y * c1 - m.y.z * c2 + m.y.w * c3) -
m.x.y * (m.y.x * c1 - m.y.z * c4 + m.y.w * c5) +
m.x.z * (m.y.x * c2 - m.y.y * c4 + m.y.w * c6) -
m.x.w * (m.y.x * c3 - m.y.y * c5 + m.y.z * c6));
}
// Construct a perspective projection matrix offset from the centre
public static m4x4 ProjectionPerspective(float l, float r, float t, float b, float zn, float zf, bool righthanded)
{
Debug.Assert(Math_.IsFinite(l) && Math_.IsFinite(r) && Math_.IsFinite(t) && Math_.IsFinite(b) && (r - l) > 0 && (t - b) > 0, "invalid view rect");
Debug.Assert(Math_.IsFinite(zn) && Math_.IsFinite(zf) && zn > 0 && zf > 0 && (zn - zf) != 0, "invalid near/far planes");
var rh = Math_.SignF(righthanded);
var mat = new m4x4 {
};
mat.x.x = 2.0f * zn / (r - l);
mat.y.y = 2.0f * zn / (t - b);
mat.z.x = rh * (r + l) / (r - l);
mat.z.y = rh * (t + b) / (t - b);
mat.z.w = -rh;
mat.z.z = rh * zf / (zn - zf);
mat.w.z = zn * zf / (zn - zf);
return(mat);
}
public static bool FEqlRelative(m4x4 a, m4x4 b, float tol)
{
var max_a = MaxElement(Abs(a));
var max_b = MaxElement(Abs(b));
if (max_b == 0)
{
return(max_a < tol);
}
if (max_a == 0)
{
return(max_b < tol);
}
var abs_max_element = Max(max_a, max_b);
return(FEqlAbsolute(a, b, tol * abs_max_element));
}
public Txfm6x8(m4x4 a2b)
: this()
{
this.a2b = a2b;
}
/// <summary>Return 'm' inverted</summary>
public static m4x4 Invert(m4x4 m)
{
Debug.Assert(IsInvertible(m), "Matrix has no inverse");
var A = Transpose(m); // Take the transpose so that row operations are faster
var B = m4x4.Identity;
// Loop through columns of 'A'
for (int j = 0; j != 4; ++j)
{
// Select the pivot element: maximum magnitude in this row
var pivot = 0; var val = 0f;
if (j <= 0 && val < Math.Abs(A.x[j]))
{
pivot = 0; val = Math.Abs(A.x[j]);
}
if (j <= 1 && val < Math.Abs(A.y[j]))
{
pivot = 1; val = Math.Abs(A.y[j]);
}
if (j <= 2 && val < Math.Abs(A.z[j]))
{
pivot = 2; val = Math.Abs(A.z[j]);
}
if (j <= 3 && val < Math.Abs(A.w[j]))
{
pivot = 3; val = Math.Abs(A.w[j]);
}
if (val < TinyF)
{
Debug.Assert(false, "Matrix has no inverse");
return(m);
}
// Interchange rows to put pivot element on the diagonal
if (pivot != j) // skip if already on diagonal
{
var a = A[j]; A[j] = A[pivot]; A[pivot] = a;
var b = B[j]; B[j] = B[pivot]; B[pivot] = b;
}
// Divide row by pivot element. Pivot element becomes 1.0f
var scale = A[j][j];
A[j] /= scale;
B[j] /= scale;
// Subtract this row from others to make the rest of column j zero
if (j != 0)
{
scale = A.x[j]; A.x -= scale * A[j]; B.x -= scale * B[j];
}
if (j != 1)
{
scale = A.y[j]; A.y -= scale * A[j]; B.y -= scale * B[j];
}
if (j != 2)
{
scale = A.z[j]; A.z -= scale * A[j]; B.z -= scale * B[j];
}
if (j != 3)
{
scale = A.w[j]; A.w -= scale * A[j]; B.w -= scale * B[j];
}
}
// When these operations have been completed, A should have been transformed to the identity matrix
// and B should have been transformed into the inverse of the original A
B = Transpose(B);
return(B);
}
/// <summary>Return 'm' inverted (assuming an orthonormal matrix</summary>
public static m4x4 InvertFast(m4x4 m)
{
InvertFast(ref m);
return(m);
}
/// <summary>True if 'mat' has an inverse</summary>
public static bool IsInvertible(m4x4 m)
{
return(Determinant(m) != 0);
}
public Spline(m4x4 spline)
{
Debug.Assert(spline.x.w == 1.0f && spline.y.w == 1.0f && spline.z.w == 1.0f && spline.w.w == 1.0f, "Splines are constructed from 4 positions");
m = spline;
}
public static bool FEql(m4x4 lhs, m4x4 rhs)
{
return(FEqlRelative(lhs, rhs, TinyF));
}
/// <summary>True if the rotation part of this transform is orthonormal</summary>
public static bool IsOrthonormal(m4x4 m)
{
return(IsOrthonormal(m.rot));
}
public static m4x4 Orthonormalise(m4x4 m)
{
Orthonormalise(ref m);
return(m);
}
/// <summary>Orthonormalise the rotation part of an affine transform</summary>
public static void Orthonormalise(ref m4x4 m)
{
Orthonormalise(ref m.rot);
}
public static m4x4 Transpose(m4x4 m)
{
Transpose(ref m);
return(m);
}
/// <summary>Transpose the rotation part of an affine transform</summary>
public static void Transpose3x3(ref m4x4 m)
{
Transpose(ref m.rot);
}
