public static Transform Rotate(float angle, Vector axis)
{
Vector a = Vector.Normalize(axis);
float s = MathUtility.Sin(MathUtility.ToRadians(angle));
float c = MathUtility.Cos(MathUtility.ToRadians(angle));
var m = new float[4, 4];
m[0, 0] = a.X * a.X + (1.0f - a.X * a.X) * c;
m[0, 1] = a.X * a.Y * (1.0f - c) - a.Z * s;
m[0, 2] = a.X * a.Z * (1.0f - c) + a.Y * s;
m[0, 3] = 0;
m[1, 0] = a.X * a.Y * (1.0f - c) + a.Z * s;
m[1, 1] = a.Y * a.Y + (1.0f - a.Y * a.Y) * c;
m[1, 2] = a.Y * a.Z * (1.0f - c) - a.X * s;
m[1, 3] = 0;
m[2, 0] = a.X * a.Z * (1.0f - c) - a.Y * s;
m[2, 1] = a.Y * a.Z * (1.0f - c) + a.X * s;
m[2, 2] = a.Z * a.Z + (1.0f - a.Z * a.Z) * c;
m[2, 3] = 0;
m[3, 0] = 0;
m[3, 1] = 0;
m[3, 2] = 0;
m[3, 3] = 1;
var mat = new Matrix4x4(m);
return(new Transform(mat, Matrix4x4.Transpose(mat)));
}
public static Transform RotateZ(float angle)
{
float sinT = MathUtility.Sin(MathUtility.ToRadians(angle));
float cosT = MathUtility.Cos(MathUtility.ToRadians(angle));
var m = new Matrix4x4(
cosT, -sinT, 0, 0,
sinT, cosT, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
return(new Transform(m, Matrix4x4.Transpose(m)));
}
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 Transform ToTransform()
{
float xx = V.X * V.X, yy = V.Y * V.Y, zz = V.Z * V.Z;
float xy = V.X * V.Y, xz = V.X * V.Z, yz = V.Y * V.Z;
float wx = V.X * W, wy = V.Y * W, wz = V.Z * W;
var m = new Matrix4x4();
m.M[0, 0] = 1.0f - 2.0f * (yy + zz);
m.M[0, 1] = 2.0f * (xy + wz);
m.M[0, 2] = 2.0f * (xz - wy);
m.M[1, 0] = 2.0f * (xy - wz);
m.M[1, 1] = 1.0f - 2.0f * (xx + zz);
m.M[1, 2] = 2.0f * (yz + wx);
m.M[2, 0] = 2.0f * (xz + wy);
m.M[2, 1] = 2.0f * (yz - wx);
m.M[2, 2] = 1.0f - 2.0f * (xx + yy);
// Transpose since we are left-handed. Ugh.
return(new Transform(Matrix4x4.Transpose(m), m));
}
public static Transform Transpose(Transform t)
{
return(new Transform(
Matrix4x4.Transpose(t._m),
Matrix4x4.Transpose(t._mInv)));
}
