public static Transform operator *(Transform t1, Transform t2)
{
Matrix4x4 m1 = Matrix4x4.Mul(t1._m, t2._m);
Matrix4x4 m2 = Matrix4x4.Mul(t2._mInv, t1._mInv);
return(new Transform(m1, m2));
}
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);
}