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