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