public static Transform Translate(Vector delta)
{
Matrix4x4 m = new Matrix4x4(new float[4, 4]{
{1,0,0,delta.X},
{0,1,0,delta.Y},
{0,0,1,delta.Z},
{0,0,0,1}});
Matrix4x4 minv = new Matrix4x4(new float[4, 4]{
{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 ToTransform()
{
float xx = _x * _x, yy = _y * _y, zz = _z * _z;
float xy = _x * _y, xz = _x * _z, yz = _y * _z;
float wx = _x * _w, wy = _y * _w, wz = _z * _w;
Matrix4x4 m = new Matrix4x4();
m[0, 0] = 1.0f - 2.0f * (yy + zz);
m[0, 1] = 2.0f * (xy + wz);
m[0, 2] = 2.0f * (xz - wy);
m[1, 0] = 2.0f * (xy - wz);
m[1, 1] = 1.0f - 2.0f * (xx + zz);
m[1, 2] = 2.0f * (yz + wx);
m[2, 0] = 2.0f * (xz + wy);
m[2, 1] = 2.0f * (yz - wx);
m[2, 2] = 1.0f - 2.0f * (xx + yy);
return new Transform(Matrix4x4.Transpose(m), m);
}
public static Matrix4x4 operator *(Matrix4x4 m1, Matrix4x4 m2)
{
Matrix4x4 r = new Matrix4x4();
for (int i = 0; i < 4; ++i)
for (int j = 0; j < 4; ++j)
r[i, j] = m1[i, 0] * m2[0, j] +
m1[i, 1] * m2[1, j] +
m1[i, 2] * m2[2, j] +
m1[i, 3] * m2[3, j];
return r;
}
public static Matrix4x4 Inverse(Matrix4x4 matrix)
{
matrix.Inverse();
return matrix;
}
public static Matrix4x4 Transpose(Matrix4x4 matrix)
{
return new Matrix4x4(new float[,] {
{matrix[0,0], matrix[1,0], matrix[2,0], matrix[3,0]},
{matrix[0,1], matrix[1,1], matrix[2,1], matrix[3,1]},
{matrix[0,2], matrix[1,2], matrix[2,2], matrix[3,2]},
{matrix[0,3], matrix[1,3], matrix[2,3], matrix[3,3]}});
}
public static Transform Scale(float x, float y, float z)
{
Matrix4x4 m = new Matrix4x4(new float[4, 4]{
{x,0,0,0},
{0,y,0,0},
{0,0,z,0},
{0,0,0,1}});
Matrix4x4 minv = new Matrix4x4(new float[4, 4]{
{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 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);
}
public void Interpolate(float time, Transform t)
{
if (!actuallyAnimated || time <= startTime)
{
t = startTransform;
return;
}
if (time >= endTime)
{
t = endTransform;
return;
}
float dt = (time - startTime) / (endTime - startTime);
Vector trans = (1.0f - dt) * T[0] + dt * T[1];
Quaternion rotate = Quaternion.Slerp(dt, R[0], R[1]);
Matrix4x4 scale = new Matrix4x4();
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
scale[i, j] = MathHelper.Lerp(dt, S[0][i, j], S[1][i, j]);
t = Transform.Translate(trans) * rotate.ToTransform() * new Transform(scale);
}
public static Transform Perspective(float fov, float n, float f)
{
Matrix4x4 persp = new Matrix4x4(new float[4, 4] {
{ 1, 0, 0, 0},
{0, 1, 0, 0},
{0, 0, f / (f - n), -f * n / (f - n)},
{0, 0, 1, 0}
});
float invTanAng = 1.0f / MathHelper.Tan(MathHelper.ToRadians(fov) / 2.0f);
return Scale(invTanAng, invTanAng, 1) * new Transform(persp);
}
public void Transpose()
{
mat = Matrix4x4.Transpose(mat);
minv = Matrix4x4.Transpose(minv);
}
public static Transform LookAt(Point pos, Point target, Vector up)
{
float[,] m = new float[4, 4];
m[0, 3] = pos.X;
m[1, 3] = pos.Y;
m[2, 3] = pos.Z;
m[3, 3] = 1;
Vector dir = Vector.Normalize(target - 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;
Matrix4x4 camToWorld = new Matrix4x4(m);
return new Transform(Matrix4x4.Inverse(camToWorld), camToWorld);
}
public Transform(Matrix4x4 m, Matrix4x4 minv)
{
mat = m;
this.minv = minv;
}
public static Transform Rotate(float angle, Vector axis)
{
Vector a = Vector.Normalize(axis);
float s = MathHelper.Sin(MathHelper.ToRadians(angle));
float c = MathHelper.Cos(MathHelper.ToRadians(angle));
float[,] 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;
Matrix4x4 mat = new Matrix4x4(m);
return new Transform(mat, Matrix4x4.Transpose(mat));
}
public static Transform RotateZ(float angle)
{
float sin_t = MathHelper.Sin(MathHelper.ToRadians(angle));
float cos_t = MathHelper.Cos(MathHelper.ToRadians(angle));
Matrix4x4 m = new Matrix4x4(new float[4, 4]{
{cos_t,-sin_t,0,0},
{sin_t,cos_t,0,0},
{0,0,1,0},
{0,0,0,1}});
return new Transform(m, Matrix4x4.Transpose(m));
}
public Transform(Matrix4x4 m)
{
mat = m;
minv = Matrix4x4.Inverse(m);
}