public Transform(float[,] mat)
{
this.m = new Matrix4x4(mat[0, 0], mat[0, 1], mat[0, 2], mat[0, 3],
mat[1, 0], mat[1, 1], mat[1, 2], mat[1, 3],
mat[2, 0], mat[2, 1], mat[2, 2], mat[2, 3],
mat[3, 0], mat[3, 1], mat[3, 2], mat[3, 3]);
this.mInv = Inverse(this.m);
}
public static Matrix4x4 Mul(Matrix4x4 m1, Matrix4x4 m2)
{
Matrix4x4 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 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;
Matrix4x4 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(Transform.Transpose(m), m);
}
public static Matrix4x4 Inverse(Matrix4x4 m)
{
int[] indxc = new int[4];
int[] indxr = new int[4];
int[] ipiv = { 0, 0, 0, 0 };
float[,] minv = new float[4, 4];
Array.Copy(m.m, minv, 4 * 4);
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 = (float)(Math.Abs(minv[j, k]));
irow = j;
icol = k;
}
}
else if (ipiv[k] > 1)
throw new Exception("Singular matrix in MatrixInvert");
}
}
}
++ipiv[icol];
// Swap rows _irow_ and _icol_ for pivot
if (irow != icol)
{
for (int k = 0; k < 4; ++k)
Utility.Swap<float>(ref minv[irow, k], ref minv[icol, k]);
}
indxr[i] = irow;
indxc[i] = icol;
if (minv[icol, icol] == 0.0f)
throw new Exception("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++)
Utility.Swap<float>(ref minv[k, indxr[j]], ref minv[k, indxc[j]]);
}
}
return new Matrix4x4(minv);
}
public Transform(Matrix4x4 mat, Matrix4x4 minv)
{
this.m = mat;
this.mInv = minv;
}
public Transform(Matrix4x4 mat)
{
this.m = mat;
this.mInv = Inverse(mat);
}
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]);
}
public static Transform Translate(Vector delta)
{
Matrix4x4 m = new Matrix4x4(1, 0, 0, delta.x,
0, 1, 0, delta.y,
0, 0, 1, delta.z,
0, 0, 0, 1);
Matrix4x4 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 static Transform Scale(float x, float y, float z)
{
Matrix4x4 m = new Matrix4x4(x, 0, 0, 0,
0, y, 0, 0,
0, 0, z, 0,
0, 0, 0, 1);
Matrix4x4 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 RotateZ(float angle)
{
float sin_t = (float)Math.Sin(Utility.Radians(angle));
float cos_t = (float)Math.Cos(Utility.Radians(angle));
Matrix4x4 m = new Matrix4x4(cos_t, -sin_t, 0, 0,
sin_t, cos_t, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
return new Transform(m, Transpose(m));
}
public static Transform Rotate(float angle, Vector axis)
{
Vector a = Geometry.Geometry.Normalize(axis);
float s = (float)Math.Sin(Utility.Radians(angle));
float c = (float)Math.Cos(Utility.Radians(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, Transpose(mat));
}
public static Transform LookAt(Point pos, Point look, Vector up)
{
float[,] 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 = Geometry.Geometry.Normalize(look - pos);
Vector left = Geometry.Geometry.Normalize(Geometry.Geometry.Cross(Geometry.Geometry.Normalize(up), dir));
Vector newUp = Geometry.Geometry.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(Inverse(camToWorld), camToWorld);
}
