public static Matrix4x4 Transpose(Matrix4x4 m)
{
return new Matrix4x4(
m.M[0, 0], m.M[1, 0], m.M[2, 0], m.M[3, 0],
m.M[0, 1], m.M[1, 1], m.M[2, 1], m.M[3, 1],
m.M[0, 2], m.M[1, 2], m.M[2, 2], m.M[3, 2],
m.M[0, 3], m.M[1, 3], m.M[2, 3], m.M[3, 3]);
}
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 void Interpolate(float time, out Transform t)
{
// Handle boundary conditions for matrix interpolation
if (!_actuallyAnimated || time <= _startTime)
{
t = _startTransform;
return;
}
if (time >= _endTime)
{
t = _endTransform;
return;
}
float dt = (time - _startTime) / (_endTime - _startTime);
// Interpolate translation at _dt_
Vector trans = (1.0f - dt) * _t[0] + dt * _t[1];
// Interpolate rotation at _dt_
Quaternion rotate = Quaternion.Slerp(dt, _r[0], _r[1]);
// Interpolate scale at _dt_
var scale = new Matrix4x4();
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
scale.M[i, j] = MathUtility.Lerp(dt, _s[0].M[i, j], _s[1].M[i, j]);
// Compute interpolated matrix as product of interpolated components
t = Transform.Translate(trans) * rotate.ToTransform() * new Transform(scale);
}
public static Matrix4x4 Invert(Matrix4x4 m)
{
int[] indxc = new int[4], indxr = new int[4];
int[] ipiv = { 0, 0, 0, 0 };
var minv = new float[4, 4];
Array.Copy(m.M, minv, m.M.LongLength);
for (int i = 0; i < 4; i++)
{
int irow = -1, icol = -1;
float big = 0.0f;
// Choose pivot
for (int j = 0; j < 4; j++)
if (ipiv[j] != 1)
for (int k = 0; k < 4; k++)
if (ipiv[k] == 0)
{
if (Math.Abs(minv[j, k]) >= big)
{
big = Math.Abs(minv[j, k]);
irow = j;
icol = k;
}
}
else if (ipiv[k] > 1)
throw new InvalidOperationException("Singular matrix in MatrixInvert");
++ipiv[icol];
// Swap rows _irow_ and _icol_ for pivot
if (irow != icol)
{
for (int k = 0; k < 4; ++k)
MathUtility.Swap(ref minv[irow, k], ref minv[icol, k]);
}
indxr[i] = irow;
indxc[i] = icol;
if (minv[icol, icol] == 0.0f)
throw new InvalidOperationException("Singular matrix in MatrixInvert");
// Set $m[icol][icol]$ to one by scaling row _icol_ appropriately
float pivinv = 1.0f / minv[icol, icol];
minv[icol, icol] = 1.0f;
for (int j = 0; j < 4; j++)
minv[icol, j] *= pivinv;
// Subtract this row from others to zero out their columns
for (int j = 0; j < 4; j++)
if (j != icol)
{
float save = minv[j, icol];
minv[j, icol] = 0;
for (int k = 0; k < 4; k++)
minv[j, k] -= minv[icol, k] * save;
}
}
// Swap columns to reflect permutation
for (int j = 3; j >= 0; j--)
{
if (indxr[j] != indxc[j])
{
for (int k = 0; k < 4; k++)
MathUtility.Swap(ref minv[k, indxr[j]], ref minv[k, indxc[j]]);
}
}
return new Matrix4x4(minv);
}
public static Matrix4x4 Mul(Matrix4x4 m1, Matrix4x4 m2)
{
var r = new Matrix4x4();
for (int i = 0; i < 4; ++i)
for (int j = 0; j < 4; ++j)
r.M[i,j] =
m1.M[i,0] * m2.M[0,j] +
m1.M[i,1] * m2.M[1,j] +
m1.M[i,2] * m2.M[2,j] +
m1.M[i,3] * m2.M[3,j];
return r;
}
public static Transform LookAt(Point pos, Point look, Vector up)
{
var m = new float[4, 4];
// Initialize fourth column of viewing matrix
m[0, 3] = pos.X;
m[1, 3] = pos.Y;
m[2, 3] = pos.Z;
m[3, 3] = 1;
// Initialize first three columns of viewing matrix
Vector dir = Vector.Normalize(look - pos);
Vector left = Vector.Normalize(Vector.Cross(Vector.Normalize(up), dir));
Vector newUp = Vector.Cross(dir, left);
m[0, 0] = left.X;
m[1, 0] = left.Y;
m[2, 0] = left.Z;
m[3, 0] = 0.0f;
m[0, 1] = newUp.X;
m[1, 1] = newUp.Y;
m[2, 1] = newUp.Z;
m[3, 1] = 0.0f;
m[0, 2] = dir.X;
m[1, 2] = dir.Y;
m[2, 2] = dir.Z;
m[3, 2] = 0.0f;
var camToWorld = new Matrix4x4(m);
return new Transform(Matrix4x4.Invert(camToWorld), camToWorld);
}
public static Transform Perspective(float fov, float n, float f)
{
// Perform projective divide
var persp = new Matrix4x4(
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, f / (f - n), -f * n / (f - n),
0, 0, 1, 0);
// Scale to canonical viewing volume
float invTanAng = 1.0f / MathUtility.Tan(MathUtility.ToRadians(fov) / 2.0f);
return Scale(invTanAng, invTanAng, 1) * new Transform(persp);
}
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));
}
public static Transform Scale(float x, float y, float z)
{
var m = new Matrix4x4(
x, 0, 0, 0,
0, y, 0, 0,
0, 0, z, 0,
0, 0, 0, 1);
var minv = new Matrix4x4(
1.0f / x, 0, 0, 0,
0, 1.0f / y, 0, 0,
0, 0, 1.0f / z, 0,
0, 0, 0, 1);
return new Transform(m, minv);
}
public static Transform Translate(Vector delta)
{
var m = new Matrix4x4(
1, 0, 0, delta.X,
0, 1, 0, delta.Y,
0, 0, 1, delta.Z,
0, 0, 0, 1);
var minv = new Matrix4x4(
1, 0, 0, -delta.X,
0, 1, 0, -delta.Y,
0, 0, 1, -delta.Z,
0, 0, 0, 1);
return new Transform(m, minv);
}
public Transform(Matrix4x4 mat, Matrix4x4 minv = null)
{
_m = mat;
_mInv = minv ?? Matrix4x4.Invert(_m);
}