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