/// <summary>Transforms the given matrix by applying the given Quaternion rotation.</summary>
/// <param name="value">The source matrix to transform.</param>
/// <param name="rotation">The rotation to apply.</param>
/// <returns>The transformed matrix.</returns>
public static Matrix3X3 <T> Transform <T>(Matrix3X3 <T> value, Quaternion <T> rotation)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
// Compute rotation matrix.
T x2 = Scalar.Add(rotation.X, rotation.X);
T y2 = Scalar.Add(rotation.Y, rotation.Y);
T z2 = Scalar.Add(rotation.Z, rotation.Z);
T wx2 = Scalar.Multiply(rotation.W, x2);
T wy2 = Scalar.Multiply(rotation.W, y2);
T wz2 = Scalar.Multiply(rotation.W, z2);
T xx2 = Scalar.Multiply(rotation.X, x2);
T xy2 = Scalar.Multiply(rotation.X, y2);
T xz2 = Scalar.Multiply(rotation.X, z2);
T yy2 = Scalar.Multiply(rotation.Y, y2);
T yz2 = Scalar.Multiply(rotation.Y, z2);
T zz2 = Scalar.Multiply(rotation.Z, z2);
T q11 = Scalar.Subtract(Scalar.Subtract(Scalar <T> .One, yy2), zz2);
T q21 = Scalar.Subtract(xy2, wz2);
T q31 = Scalar.Add(xz2, wy2);
T q12 = Scalar.Add(xy2, wz2);
T q22 = Scalar.Subtract(Scalar.Subtract(Scalar <T> .One, xx2), zz2);
T q32 = Scalar.Subtract(yz2, wx2);
T q13 = Scalar.Subtract(xz2, wy2);
T q23 = Scalar.Add(yz2, wx2);
T q33 = Scalar.Subtract(Scalar.Subtract(Scalar <T> .One, xx2), yy2);
var q1 = new Vector3D <T>(q11, q12, q13);
var q2 = new Vector3D <T>(q21, q22, q23);
var q3 = new Vector3D <T>(q31, q32, q33);
return(new(value.M11 * q1 + value.M12 * q2 + value.M13 * q3, value.M21 *q1 + value.M22 * q2 + value.M23 * q3, value.M31 *q1 + value.M32 * q2 + value.M33 * q3));
}
public static Matrix4X3 <T> Multiply <T>(Matrix4X3 <T> value1, Matrix3X3 <T> value2)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
=> value1 * value2;
/// <summary>Transposes the rows and columns of a matrix.</summary>
/// <param name="matrix">The source matrix.</param>
/// <returns>The transposed matrix.</returns>
public static unsafe Matrix3X3 <T> Transpose <T>(Matrix3X3 <T> matrix)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
return(new(matrix.Column1, matrix.Column2, matrix.Column3));
}
public static Matrix3X3 <T> Add <T>(Matrix3X3 <T> value1, Matrix3X3 <T> value2)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
{
return(value1 + value2);
}
/// <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);
}
public static Matrix3X3 <T> Subtract <T>(Matrix3X3 <T> value1, Matrix3X3 <T> value2)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
=> value1 - value2;
public static Matrix3X3 <T> Negate <T>(Matrix3X3 <T> value)
where T : unmanaged, IFormattable, IEquatable <T>, IComparable <T>
=> - value;
