/// <summary>Attempts to invert the given matrix. If the operation succeeds, the inverted matrix is stored in the result parameter.</summary> /// <param name="matrix">The source matrix.</param> /// <param name="result">The output matrix.</param> /// <returns>True if the operation succeeded, False otherwise.</returns> public static bool Invert <T>(Matrix3X2 <T> matrix, out Matrix3X2 <T> result) where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T> { T det = Scalar.Subtract(Scalar.Multiply(matrix.M11, matrix.M22), Scalar.Multiply(matrix.M21, matrix.M12)); if (!Scalar.GreaterThanOrEqual(Scalar.Abs(det), Scalar <T> .Epsilon)) { result = new(Scalar <T> .NaN, Scalar <T> .NaN, Scalar <T> .NaN, Scalar <T> .NaN, Scalar <T> .NaN, Scalar <T> .NaN); return(false); } T invDet = Scalar.Reciprocal(det); result = default; result.M11 = Scalar.Multiply(matrix.M22, invDet); result.M12 = Scalar.Negate(Scalar.Multiply(matrix.M12, invDet)); result.M21 = Scalar.Negate(Scalar.Multiply(matrix.M21, invDet)); result.M22 = Scalar.Multiply(matrix.M11, invDet); result.M31 = Scalar.Multiply(Scalar.Subtract(Scalar.Multiply(matrix.M21, matrix.M32), Scalar.Multiply(matrix.M31, matrix.M22)), invDet); result.M32 = Scalar.Multiply(Scalar.Subtract(Scalar.Multiply(matrix.M31, matrix.M12), Scalar.Multiply(matrix.M11, matrix.M32)), invDet); return(true); }
public static Plane <T> Normalize <T>(Plane <T> value) where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T> { /*if (Vector.IsHardwareAccelerated) * { * T normalLengthSquared = value.Normal.LengthSquared(); * if (MathF.Abs(normalLengthSquared - 1.0f) < NormalizeEpsilon) * { * // It already normalized, so we don't need to farther process. * return value; * } * T normalLength = MathF.Sqrt(normalLengthSquared); * return new Plane( * value.Normal / normalLength, * value.D / normalLength); * } * else*/ { T f = Scalar.Add( Scalar.Add(Scalar.Multiply(value.Normal.X, value.Normal.X), Scalar.Multiply(value.Normal.Y, value.Normal.Y)), Scalar.Multiply(value.Normal.Z, value.Normal.Z)); if (!Scalar.GreaterThanOrEqual(Scalar.Abs(Scalar.Subtract(f, Scalar <T> .One)), Scalar.As <float, T>(NormalizeEpsilon))) { return(value); // It already normalized, so we don't need to further process. } T fInv = Scalar.Reciprocal(Scalar.Sqrt(f)); return(new( Scalar.Multiply(value.Normal.X, fInv), Scalar.Multiply(value.Normal.Y, fInv), Scalar.Multiply(value.Normal.Z, fInv), Scalar.Multiply(value.Distance, fInv))); } }
/// <summary>Attempts to extract the scale, translation, and rotation components from the given scale/rotation/translation matrix. /// If successful, the out parameters will contained the extracted values.</summary> /// <param name="matrix">The source matrix.</param> /// <param name="scale">The scaling component of the transformation matrix.</param> /// <param name="rotation">The rotation component of the transformation matrix.</param> /// <returns>True if the source matrix was successfully decomposed; False otherwise.</returns> public static bool Decompose <T>(Matrix3X3 <T> matrix, out Vector3D <T> scale, out Quaternion <T> rotation) where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T> { bool result = true; unsafe { fixed(Vector3D <T> *scaleBase = &scale) { T *pfScales = (T *)scaleBase; T det; VectorBasis <T> vectorBasis; Vector3D <T> ** pVectorBasis = (Vector3D <T> **) & vectorBasis; Matrix3X3 <T> matTemp = Matrix3X3 <T> .Identity; CanonicalBasis <T> canonicalBasis = default; Vector3D <T> * pCanonicalBasis = &canonicalBasis.Row0; canonicalBasis.Row0 = new Vector3D <T>(Scalar <T> .One, Scalar <T> .Zero, Scalar <T> .Zero); canonicalBasis.Row1 = new Vector3D <T>(Scalar <T> .Zero, Scalar <T> .One, Scalar <T> .Zero); canonicalBasis.Row2 = new Vector3D <T>(Scalar <T> .Zero, Scalar <T> .Zero, Scalar <T> .One); pVectorBasis[0] = &matTemp.Row1; pVectorBasis[1] = &matTemp.Row2; pVectorBasis[2] = &matTemp.Row3; *(pVectorBasis[0]) = new Vector3D <T>(matrix.M11, matrix.M12, matrix.M13); *(pVectorBasis[1]) = new Vector3D <T>(matrix.M21, matrix.M22, matrix.M23); *(pVectorBasis[2]) = new Vector3D <T>(matrix.M31, matrix.M32, matrix.M33); scale.X = pVectorBasis[0]->Length; scale.Y = pVectorBasis[1]->Length; scale.Z = pVectorBasis[2]->Length; uint a, b, c; #region Ranking T x = pfScales[0], y = pfScales[1], z = pfScales[2]; if (!Scalar.GreaterThanOrEqual(x, y)) { if (!Scalar.GreaterThanOrEqual(y, z)) { a = 2; b = 1; c = 0; } else { a = 1; if (!Scalar.GreaterThanOrEqual(x, z)) { b = 2; c = 0; } else { b = 0; c = 2; } } } else { if (!Scalar.GreaterThanOrEqual(x, z)) { a = 2; b = 0; c = 1; } else { a = 0; if (!Scalar.GreaterThanOrEqual(y, z)) { b = 2; c = 1; } else { b = 1; c = 2; } } } #endregion if (!Scalar.GreaterThanOrEqual(pfScales[a], Scalar.As <float, T>(DecomposeEpsilon))) { *(pVectorBasis[a]) = pCanonicalBasis[a]; } *pVectorBasis[a] = Vector3D.Normalize(*pVectorBasis[a]); if (!Scalar.GreaterThanOrEqual(pfScales[b], Scalar.As <float, T>(DecomposeEpsilon))) { uint cc; T fAbsX, fAbsY, fAbsZ; fAbsX = Scalar.Abs(pVectorBasis[a]->X); fAbsY = Scalar.Abs(pVectorBasis[a]->Y); fAbsZ = Scalar.Abs(pVectorBasis[a]->Z); #region Ranking if (!Scalar.GreaterThanOrEqual(fAbsX, fAbsY)) { if (!Scalar.GreaterThanOrEqual(fAbsY, fAbsZ)) { cc = 0; } else { if (!Scalar.GreaterThanOrEqual(fAbsX, fAbsZ)) { cc = 0; } else { cc = 2; } } } else { if (!Scalar.GreaterThanOrEqual(fAbsX, fAbsZ)) { cc = 1; } else { if (!Scalar.GreaterThanOrEqual(fAbsY, fAbsZ)) { cc = 1; } else { cc = 2; } } } #endregion *pVectorBasis[b] = Vector3D.Cross(*pVectorBasis[a], *(pCanonicalBasis + cc)); } *pVectorBasis[b] = Vector3D.Normalize(*pVectorBasis[b]); if (!Scalar.GreaterThanOrEqual(pfScales[c], Scalar.As <float, T>(DecomposeEpsilon))) { *pVectorBasis[c] = Vector3D.Cross(*pVectorBasis[a], *pVectorBasis[b]); } *pVectorBasis[c] = Vector3D.Normalize(*pVectorBasis[c]); det = matTemp.GetDeterminant(); // use Kramer's rule to check for handedness of coordinate system if (!Scalar.GreaterThanOrEqual(det, Scalar <T> .Zero)) { // switch coordinate system by negating the scale and inverting the basis vector on the x-axis pfScales[a] = Scalar.Negate(pfScales[a]); *pVectorBasis[a] = -(*pVectorBasis[a]); det = Scalar.Negate(det); } det = Scalar.Subtract(det, Scalar <T> .One); det = Scalar.Multiply(det, det); if (!Scalar.GreaterThanOrEqual(Scalar.As <float, T>(DecomposeEpsilon), det)) { // Non-SRT matrix encountered rotation = Quaternion <T> .Identity; result = false; } else { // generate the quaternion from the matrix rotation = Quaternion <T> .CreateFromRotationMatrix(matTemp); } } } return(result); }