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