// 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); }