/// <summary>
/// M = TRS ,get TRS
/// </summary>
/// <param name="m"></param>
/// <param name="T"></param>
/// <param name="Rquat"></param>
/// <param name="S"></param>
public void Decompose(Matrix4x4 m, out Vector T, out Quaternion Rquat, out Matrix4x4 S)
{
T = new Vector();
// Rquat = new Quaternion();
//S = new Matrix4x4();
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 = new Matrix4x4(m);
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 = new Matrix4x4(M);
do
{
// Compute next matrix _Rnext_ in series
Matrix4x4 Rnext = new Matrix4x4();
Matrix4x4 Rit = LR.Inverse(LR.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 = (float)Math.Abs(R.m[i, 0] - Rnext.m[i, 0]) +
(float)Math.Abs(R.m[i, 1] - Rnext.m[i, 1]) +
(float)Math.Abs(R.m[i, 2] - Rnext.m[i, 2]);
norm = (float)Math.Max(norm, n);
}
R = Rnext;
} while (++count < 100 && norm > .0001f);
// XXX TODO FIXME deal with flip...
Rquat = new Quaternion(new Transform(R));
// Compute scale _S_ using rotation and original matrix
S = LR.Multiply(LR.Inverse(R), M);
}
public Transform(Matrix4x4 M)
{
m = M;
mInv = LR.Inverse(M);
}