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