private static void Decompose(Matrix4x4 m, out Vector t, out Quaternion r, out Matrix4x4 s)
{
// Extract translation _T_ from transformation matrix
t.X = m.M[0, 3];
t.Y = m.M[1, 3];
t.Z = m.M[2, 3];
// Compute new transformation matrix _M_ without translation
Matrix4x4 M = m.Clone();
for (int i = 0; i < 3; ++i)
M.M[i, 3] = M.M[3, i] = 0.0f;
M.M[3, 3] = 1.0f;
// Extract rotation _R_ from transformation matrix
float norm;
int count = 0;
Matrix4x4 R = M.Clone();
do
{
// Compute next matrix _Rnext_ in series
var Rnext = new Matrix4x4();
var Rit = Matrix4x4.Invert(Matrix4x4.Transpose(R));
for (int i = 0; i < 4; ++i)
for (int j = 0; j < 4; ++j)
Rnext.M[i, j] = 0.5f * (R.M[i, j] + Rit.M[i, j]);
// Compute norm of difference between _R_ and _Rnext_
norm = 0.0f;
for (int i = 0; i < 3; ++i)
{
float n = Math.Abs(R.M[i, 0] - Rnext.M[i, 0]) +
Math.Abs(R.M[i, 1] - Rnext.M[i, 1]) +
Math.Abs(R.M[i, 2] - Rnext.M[i, 2]);
norm = Math.Max(norm, n);
}
R = Rnext;
} while (++count < 100 && norm > .0001f);
// XXX TODO FIXME deal with flip...
r = (Quaternion) new Transform(R);
// Compute scale _S_ using rotation and original matrix
s = Matrix4x4.Mul(Matrix4x4.Invert(R), M);
}
public static float Dot(Quaternion q1, Quaternion q2)
{
return Vector.Dot(q1.V, q2.V) + q1.W * q2.W;
}
public static Quaternion Normalize(Quaternion q)
{
return q / MathUtility.Sqrt(Dot(q, q));
}
public static Quaternion Slerp(float t, Quaternion q1, Quaternion q2)
{
float cosTheta = Dot(q1, q2);
if (cosTheta > .9995f)
return Normalize((1.0f - t) * q1 + t * q2);
float theta = MathUtility.Acos(MathUtility.Clamp(cosTheta, -1.0f, 1.0f));
float thetap = theta * t;
Quaternion qperp = Normalize(q2 - q1 * cosTheta);
return q1 * MathUtility.Cos(thetap) + qperp * MathUtility.Sin(thetap);
}