public Matrix4 ConvertToW()
{
Matrix4 result = new Matrix4();
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
result[i, j] = entry[Matrix4.I(i, j)];
// magical W value
result.entry[15] = 1;
return result;
}
public Vector4 Mult(Matrix4 matrix)
{
return new Vector4(
tuple[0] * matrix.entry[ 0] + tuple[1] * matrix.entry[ 1]
+ tuple[2] * matrix.entry[ 2] + tuple[3] * matrix.entry[ 3],
tuple[0] * matrix.entry[ 4] + tuple[1] * matrix.entry[ 5]
+ tuple[2] * matrix.entry[ 6] + tuple[3] * matrix.entry[ 7],
tuple[0] * matrix.entry[ 8] + tuple[1] * matrix.entry[ 9]
+ tuple[2] * matrix.entry[10] + tuple[3] * matrix.entry[11],
tuple[0] * matrix.entry[12] + tuple[1] * matrix.entry[13]
+ tuple[2] * matrix.entry[14] + tuple[3] * matrix.entry[15]);
}
public Matrix4 Transpose()
{
Matrix4 result = new Matrix4();
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
result.entry[I(i, j)] += entry[I(j, i)];
return result;
}
public Matrix4 TransposeTimes(Matrix4 matrix)
{
// P = A^T*B, P[r][c] = sum_m A[m][r]*B[m][c]
Matrix4 result = new Matrix4();
for (int row = 0; row < 4; row++)
{
for (int col = 0; col < 4; col++)
{
int i = I(row, col);
result.entry[i] = 0;
for (int mid = 0; mid < 4; mid++)
result.entry[i] += entry[I(mid, row)] * matrix.entry[I(mid, col)];
}
}
return result;
}
public Matrix4 Negative()
{
Matrix4 result = new Matrix4();
for (int i = 0; i < 16; i++)
result.entry[i] = -entry[i];
return result;
}
public Matrix4 Sub(Matrix4 matrix)
{
Matrix4 result = new Matrix4();
for (int i = 0; i < 16; i++)
result.entry[i] = entry[i] - matrix.entry[i];
return result;
}
public Matrix4 Mult(double scalar)
{
Matrix4 result = new Matrix4();
for (int i = 0; i < 16; i++)
result.entry[i] = entry[i] * scalar;
return result;
}
public Matrix4 Mult(Matrix4 matrix)
{
Matrix4 result = new Matrix4();
for (int row = 0; row < 4; row++)
{
for (int col = 0; col < 4; col++)
{
int i = I(row, col);
result.entry[i] = 0;
for (int mid = 0; mid < 4; mid++)
result.entry[i] += entry[I(row, mid)] * matrix.entry[I(mid, col)];
}
}
return result;
}
public Matrix4 Inverse()
{
double a0 = entry[ 0] * entry[ 5] - entry[ 1] * entry[ 4];
double a1 = entry[ 0] * entry[ 6] - entry[ 2] * entry[ 4];
double a2 = entry[ 0] * entry[ 7] - entry[ 3] * entry[ 4];
double a3 = entry[ 1] * entry[ 6] - entry[ 2] * entry[ 5];
double a4 = entry[ 1] * entry[ 7] - entry[ 3] * entry[ 5];
double a5 = entry[ 2] * entry[ 7] - entry[ 3] * entry[ 6];
double b0 = entry[ 8] * entry[13] - entry[ 9] * entry[12];
double b1 = entry[ 8] * entry[14] - entry[10] * entry[12];
double b2 = entry[ 8] * entry[15] - entry[11] * entry[12];
double b3 = entry[ 9] * entry[14] - entry[10] * entry[13];
double b4 = entry[ 9] * entry[15] - entry[11] * entry[13];
double b5 = entry[10] * entry[15] - entry[11] * entry[14];
double det = a0 * b5 - a1 * b4 + a2 * b3 + a3 * b2 - a4 * b1 + a5 * b0;
if (det <= 0)
return Matrix4.Zero;
Matrix4 result = new Matrix4();
result.entry[ 0] = + entry[ 5] * b5 - entry[ 6] * b4 + entry[ 7] * b3;
result.entry[ 4] = - entry[ 4] * b5 + entry[ 6] * b2 - entry[ 7] * b1;
result.entry[ 8] = + entry[ 4] * b4 - entry[ 5] * b2 + entry[ 7] * b0;
result.entry[12] = - entry[ 4] * b3 + entry[ 5] * b1 - entry[ 6] * b0;
result.entry[ 1] = - entry[ 1] * b5 + entry[ 2] * b4 - entry[ 3] * b3;
result.entry[ 5] = + entry[ 0] * b5 - entry[ 2] * b2 + entry[ 3] * b1;
result.entry[ 9] = - entry[ 0] * b4 + entry[ 1] * b2 - entry[ 3] * b0;
result.entry[13] = + entry[ 0] * b3 - entry[ 1] * b1 + entry[ 2] * b0;
result.entry[ 2] = + entry[13] * a5 - entry[14] * a4 + entry[15] * a3;
result.entry[ 6] = - entry[12] * a5 + entry[14] * a2 - entry[15] * a1;
result.entry[10] = + entry[12] * a4 - entry[13] * a2 + entry[15] * a0;
result.entry[14] = - entry[12] * a3 + entry[13] * a1 - entry[14] * a0;
result.entry[ 3] = - entry[ 9] * a5 + entry[10] * a4 - entry[11] * a3;
result.entry[ 7] = + entry[ 8] * a5 - entry[10] * a2 + entry[11] * a1;
result.entry[11] = - entry[ 8] * a4 + entry[ 9] * a2 - entry[11] * a0;
result.entry[15] = + entry[ 8] * a3 - entry[ 9] * a1 + entry[10] * a0;
double invDet = 1 / det;
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
result[i, j] *= invDet;
return result;
}
public Matrix4 Div(double scalar)
{
Matrix4 result = new Matrix4();
if (scalar != 0)
{
double invScalar = 1 / scalar;
for (int i = 0; i < 16; i++)
result.entry[i] = entry[i] * invScalar;;
}
else
for (int i = 0; i < 16; i++)
result.entry[i] = double.MaxValue;
return result;
}
public Matrix4 Adjoint()
{
double a0 = entry[ 0] * entry[ 5] - entry[ 1] * entry[ 4];
double a1 = entry[ 0] * entry[ 6] - entry[ 2] * entry[ 4];
double a2 = entry[ 0] * entry[ 7] - entry[ 3] * entry[ 4];
double a3 = entry[ 1] * entry[ 6] - entry[ 2] * entry[ 5];
double a4 = entry[ 1] * entry[ 7] - entry[ 3] * entry[ 5];
double a5 = entry[ 2] * entry[ 7] - entry[ 3] * entry[ 6];
double b0 = entry[ 8] * entry[13] - entry[ 9] * entry[12];
double b1 = entry[ 8] * entry[14] - entry[10] * entry[12];
double b2 = entry[ 8] * entry[15] - entry[11] * entry[12];
double b3 = entry[ 9] * entry[14] - entry[10] * entry[13];
double b4 = entry[ 9] * entry[15] - entry[11] * entry[13];
double b5 = entry[10] * entry[15] - entry[11] * entry[14];
Matrix4 result = new Matrix4();
result.entry[ 0] = + entry[ 5] * b5 - entry[ 6] * b4 + entry[ 7] * b3;
result.entry[ 4] = - entry[ 4] * b5 + entry[ 6] * b2 - entry[ 7] * b1;
result.entry[ 8] = + entry[ 4] * b4 - entry[ 5] * b2 + entry[ 7] * b0;
result.entry[12] = - entry[ 4] * b3 + entry[ 5] * b1 - entry[ 6] * b0;
result.entry[ 1] = - entry[ 1] * b5 + entry[ 2] * b4 - entry[ 3] * b3;
result.entry[ 5] = + entry[ 0] * b5 - entry[ 2] * b2 + entry[ 3] * b1;
result.entry[ 9] = - entry[ 0] * b4 + entry[ 1] * b2 - entry[ 3] * b0;
result.entry[13] = + entry[ 0] * b3 - entry[ 1] * b1 + entry[ 2] * b0;
result.entry[ 2] = + entry[13] * a5 - entry[14] * a4 + entry[15] * a3;
result.entry[ 6] = - entry[12] * a5 + entry[14] * a2 - entry[15] * a1;
result.entry[10] = + entry[12] * a4 - entry[13] * a2 + entry[15] * a0;
result.entry[14] = - entry[12] * a3 + entry[13] * a1 - entry[14] * a0;
result.entry[ 3] = - entry[ 9] * a5 + entry[10] * a4 - entry[11] * a3;
result.entry[ 7] = + entry[ 8] * a5 - entry[10] * a2 + entry[11] * a1;
result.entry[11] = - entry[ 8] * a4 + entry[ 9] * a2 - entry[11] * a0;
result.entry[15] = + entry[ 8] * a3 - entry[ 9] * a1 + entry[10] * a0;
return result;
}
