/// <summary>
/// Checks whether a transformation has changed/moved significantly compared to the previous
/// transformation with the specified translational threshold and rotational threshold.
/// </summary>
/// <param name="matPrev">The previous transformation matrix</param>
/// <param name="matCurr">The current transformation matrix</param>
/// <param name="transThreshold">The translational threshold</param>
/// <param name="rotThreshold">The rotational threshold</param>
/// <returns></returns>
public static bool HasMovedSignificantly(Matrix matPrev, Matrix matCurr,
float transThreshold, float rotThreshold)
{
// 1st time through
if (matPrev.Equals(Matrix.Identity))
{
return(true);
}
// Test translation
if (Vector3.Distance(matPrev.Translation, matCurr.Translation) > transThreshold)
{
return(true);
}
// Test rotations
float dRollPrev, dPitchPrev, dYawPrev, dRollCurr, dPitchCurr, dYawCurr;
Vector3 dPrev = Vector3Helper.ExtractAngles(matPrev);
Vector3 dCurr = Vector3Helper.ExtractAngles(matCurr);
dPitchPrev = dPrev.X;
dYawPrev = dPrev.Y;
dRollPrev = dPrev.Z;
dPitchCurr = dCurr.X;
dYawCurr = dCurr.Y;
dRollCurr = dCurr.Z;
if (Math.Abs(dRollPrev - dRollCurr) > rotThreshold)
{
return(true);
}
if (Math.Abs(dPitchPrev - dPitchCurr) > rotThreshold)
{
return(true);
}
if (Math.Abs(dYawPrev - dYawCurr) > rotThreshold)
{
return(true);
}
// Not enough movement
return(false);
}
/// <summary>
/// Orthonormalizes a transformation matrix.
/// </summary>
/// <param name="mat"></param>
/// <returns></returns>
public static Matrix OrthonormalizeMatrix(Matrix mat)
{
Matrix m = mat;
Vector3 axisX = Vector3Helper.Get(m.M11, m.M12, m.M13);
Vector3 axisY = Vector3Helper.Get(m.M21, m.M22, m.M23);
Vector3 axisZ = Vector3Helper.Get(m.M31, m.M32, m.M33);
axisX.Normalize();
axisY.Normalize();
axisZ.Normalize();
axisZ = Vector3.Normalize(Vector3.Cross(axisX, axisY));
axisY = Vector3.Normalize(Vector3.Cross(axisZ, axisX));
axisX = Vector3.Normalize(Vector3.Cross(axisY, axisZ));
m.M11 = axisX.X; m.M12 = axisX.Y; m.M13 = axisX.Z;
m.M21 = axisY.X; m.M22 = axisY.Y; m.M23 = axisY.Z;
m.M31 = axisZ.X; m.M32 = axisZ.Y; m.M33 = axisZ.Z;
return(m);
}