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