public static void Decompose(Matrix m, ref Vector T, ref Quaternion Rquat, ref Matrix S) { T.x = m.m[3]; T.y = m.m[7]; T.z = m.m[11]; // Compute new transformation matrix _M_ without translation Matrix M = new Matrix (m); for (int i = 0; i < 3; ++i) M.m[i * 4 + 3] = M.m[12 + i] = 0.0; M.m[15] = 1.0; // Extract rotation _R_ from transformation matrix double norm; int count = 0; Matrix R = new Matrix (M); do { // Compute next matrix _Rnext_ in series Matrix Rnext = new Matrix (); Matrix Rit = R.Transposed.Inverse; for (int i = 0; i < 4; ++i) for (int j = 0; j < 4; ++j) Rnext.m[i * 4 + j] = 0.5 * (R.m[i * 4 + j] + Rit.m[i * 4 + j]); // Compute norm of difference between _R_ and _Rnext_ norm = 0.0; for (int i = 0; i < 3; ++i) { double n = Math.Abs (R.m[i * 4] - Rnext.m[i * 4]) + Math.Abs (R.m[i * 4 + 1] - Rnext.m[i * 4 + 1]) + Math.Abs (R.m[i * 4 + 2] - Rnext.m[i * 4 + 2]); norm = Math.Max (norm, n); } R = Rnext; } while (++count < 100 && norm > 0.0001); // XXX TODO FIXME deal with flip... Rquat = new Quaternion (new Transform (R)); // Compute scale _S_ using rotation and original matrix S = new Matrix (R.Inverse * M); }
public static Quaternion Slerp(double t, Quaternion q1, Quaternion q2) { double cosTheta = q1 ^ q2; if (cosTheta > 0.9995) return ((1.0 - t) * q1 + t * q2).Normalized; else { double theta = Math.Acos (Util.Clamp (cosTheta, -1.0, 1.0)); double thetap = theta * t; Quaternion qperp = (q2 - q1 * cosTheta).Normalized; return q1 * Math.Cos (thetap) + qperp * Math.Sin (thetap); } }