public float Dot(Quaternion q) { return _x * q._x + _y * q._y + _z * q._z + _w * q._w; }
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); else { float theta = MathHelper.Acos(MathHelper.Clamp(cosTheta, -1.0f, 1.0f)); float thetap = theta * t; Quaternion qperp = Normalize(q2 - q1 * cosTheta); return q1 * MathHelper.Cos(thetap) + qperp * MathHelper.Sin(thetap); } }
public static float Dot(Quaternion q1, Quaternion q2) { return q1.Dot(q2); }
public static Quaternion Normalize(Quaternion q) { return q / MathHelper.Sqrt(Dot(q, q)); }
public static void Decompose(Matrix4x4 m, out Vector T, out Quaternion Rquat, out Matrix4x4 S) { T = new Vector(); T.X = m[0, 3]; T.Y = m[1, 3]; T.Z = m[2, 3]; Matrix4x4 M = m; for (int i = 0; i < 3; ++i) M[i, 3] = M[3, i] = 0.0f; M[3, 3] = 1.0f; float norm; int count = 0; Matrix4x4 R = M; do { Matrix4x4 Rnext = new Matrix4x4(); Matrix4x4 Rit = Matrix4x4.Inverse(Matrix4x4.Transpose(R)); for (int i = 0; i < 4; ++i) for (int j = 0; j < 4; ++j) Rnext[i, j] = 0.5f * (R[i, j] + Rit[i, j]); norm = 0.0f; for (int i = 0; i < 3; ++i) { float n = MathHelper.Abs(R[i, 0] - Rnext[i, 0]) + MathHelper.Abs(R[i, 1] - Rnext[i, 1]) + MathHelper.Abs(R[i, 2] - Rnext[i, 2]); norm = MathHelper.Max(norm, n); } R = Rnext; } while (++count < 100 && norm > .0001f); Rquat = new Quaternion(new Transform(R)); S = (Matrix4x4.Inverse(R) * M); }