public static void DecomposeMatrix(ref Matrix4 transform, out Vector3 translate, out Quaternion rotate, out Vector3 scale) { Matrix3 tmp = transform.GetMatrix3(); translate = transform.Translation; scale.x = (float)Math.Sqrt(tmp.m00 * tmp.m00 + tmp.m01 * tmp.m01 + tmp.m02 * tmp.m02); scale.y = (float)Math.Sqrt(tmp.m10 * tmp.m10 + tmp.m11 * tmp.m11 + tmp.m12 * tmp.m12); scale.z = (float)Math.Sqrt(tmp.m20 * tmp.m20 + tmp.m21 * tmp.m21 + tmp.m22 * tmp.m22); if (tmp.Determinant < 0) // is this a reflecting scale? { scale.x *= -1; } tmp.m00 /= scale.x; tmp.m01 /= scale.x; tmp.m02 /= scale.x; tmp.m10 /= scale.y; tmp.m11 /= scale.y; tmp.m12 /= scale.y; tmp.m20 /= scale.z; tmp.m21 /= scale.z; tmp.m22 /= scale.z; rotate = Quaternion.Identity; rotate.FromRotationMatrix(tmp); if (Math.Abs(1.0 - rotate.Norm) > .01f) { string msg = string.Format("Probable non-uniform scale factor on rotation matrix; Norm = {0}", rotate.Norm); throw new Exception(msg); } if (Math.Abs(1.0 - rotate.Norm) > .001f) { Trace.TraceWarning("Possible non-uniform scale factor on rotation matrix"); } // rv.Normalize(); }
public static void DecomposeMatrix(ref Matrix4 transform, out Vector3 translate, out Quaternion rotate, out Vector3 scale) { Matrix3 tmp = transform.GetMatrix3(); translate = transform.Translation; scale.x = (float)Math.Sqrt(tmp.m00 * tmp.m00 + tmp.m01 * tmp.m01 + tmp.m02 * tmp.m02); scale.y = (float)Math.Sqrt(tmp.m10 * tmp.m10 + tmp.m11 * tmp.m11 + tmp.m12 * tmp.m12); scale.z = (float)Math.Sqrt(tmp.m20 * tmp.m20 + tmp.m21 * tmp.m21 + tmp.m22 * tmp.m22); if (tmp.Determinant < 0) // is this a reflecting scale? scale.x *= -1; tmp.m00 /= scale.x; tmp.m01 /= scale.x; tmp.m02 /= scale.x; tmp.m10 /= scale.y; tmp.m11 /= scale.y; tmp.m12 /= scale.y; tmp.m20 /= scale.z; tmp.m21 /= scale.z; tmp.m22 /= scale.z; rotate = Quaternion.Identity; rotate.FromRotationMatrix(tmp); if (Math.Abs(1.0 - rotate.Norm) > .01f) { string msg = string.Format("Probable non-uniform scale factor on rotation matrix; Norm = {0}", rotate.Norm); throw new Exception(msg); } if (Math.Abs(1.0 - rotate.Norm) > .001f) Trace.TraceWarning("Possible non-uniform scale factor on rotation matrix"); // rv.Normalize(); }