public Matrix4d Mult(Matrix4d a)
{
Matrix4d B = new Matrix4d();
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < 4; k++)
{
B[i, j] = B[i, j] + this.val[i, k] * a[k, j];
}
}
}
return(B);
}
public Matrix4d Inverse()
{
Matrix4d B = new Matrix4d();
double det;
#region calculate matrix element
B[0, 0] = val[1, 1] * val[2, 2] * val[3, 3] -
val[1, 1] * val[2, 3] * val[3, 2] -
val[2, 1] * val[1, 2] * val[3, 3] +
val[2, 1] * val[1, 3] * val[3, 2] +
val[3, 1] * val[1, 2] * val[2, 3] -
val[3, 1] * val[1, 3] * val[2, 2];
B[1, 0] = -val[1, 0] * val[2, 2] * val[3, 3] +
val[1, 0] * val[2, 3] * val[3, 2] +
val[2, 0] * val[1, 2] * val[3, 3] -
val[2, 0] * val[1, 3] * val[3, 2] -
val[3, 0] * val[1, 2] * val[2, 3] +
val[3, 0] * val[1, 3] * val[2, 2];
B[2, 0] = val[1, 0] * val[2, 1] * val[3, 3] -
val[1, 0] * val[2, 3] * val[3, 1] -
val[2, 0] * val[1, 1] * val[3, 3] +
val[2, 0] * val[1, 3] * val[3, 1] +
val[3, 0] * val[1, 1] * val[2, 3] -
val[3, 0] * val[1, 3] * val[2, 1];
B[3, 0] = -val[1, 0] * val[2, 1] * val[3, 2] +
val[1, 0] * val[2, 2] * val[3, 1] +
val[2, 0] * val[1, 1] * val[3, 2] -
val[2, 0] * val[1, 2] * val[3, 1] -
val[3, 0] * val[1, 1] * val[2, 2] +
val[3, 0] * val[1, 2] * val[2, 1];
B[0, 1] = -val[0, 1] * val[2, 2] * val[3, 3] +
val[0, 1] * val[2, 3] * val[3, 2] +
val[2, 1] * val[0, 2] * val[3, 3] -
val[2, 1] * val[0, 3] * val[3, 2] -
val[3, 1] * val[0, 2] * val[2, 3] +
val[3, 1] * val[0, 3] * val[2, 2];
B[1, 1] = val[0, 0] * val[2, 2] * val[3, 3] -
val[0, 0] * val[2, 3] * val[3, 2] -
val[2, 0] * val[0, 2] * val[3, 3] +
val[2, 0] * val[0, 3] * val[3, 2] +
val[3, 0] * val[0, 2] * val[2, 3] -
val[3, 0] * val[0, 3] * val[2, 2];
B[2, 1] = -val[0, 0] * val[2, 1] * val[3, 3] +
val[0, 0] * val[2, 3] * val[3, 1] +
val[2, 0] * val[0, 1] * val[3, 3] -
val[2, 0] * val[0, 3] * val[3, 1] -
val[3, 0] * val[0, 1] * val[2, 3] +
val[3, 0] * val[0, 3] * val[2, 1];
B[3, 1] = val[0, 0] * val[2, 1] * val[3, 2] -
val[0, 0] * val[2, 2] * val[3, 1] -
val[2, 0] * val[0, 1] * val[3, 2] +
val[2, 0] * val[0, 2] * val[3, 1] +
val[3, 0] * val[0, 1] * val[2, 2] -
val[3, 0] * val[0, 2] * val[2, 1];
B[0, 2] = val[0, 1] * val[1, 2] * val[3, 3] -
val[0, 1] * val[1, 3] * val[3, 2] -
val[1, 1] * val[0, 2] * val[3, 3] +
val[1, 1] * val[0, 3] * val[3, 2] +
val[3, 1] * val[0, 2] * val[1, 3] -
val[3, 1] * val[0, 3] * val[1, 2];
B[1, 2] = -val[0, 0] * val[1, 2] * val[3, 3] +
val[0, 0] * val[1, 3] * val[3, 2] +
val[1, 0] * val[0, 2] * val[3, 3] -
val[1, 0] * val[0, 3] * val[3, 2] -
val[3, 0] * val[0, 2] * val[1, 3] +
val[3, 0] * val[0, 3] * val[1, 2];
B[2, 2] = val[0, 0] * val[1, 1] * val[3, 3] -
val[0, 0] * val[1, 3] * val[3, 1] -
val[1, 0] * val[0, 1] * val[3, 3] +
val[1, 0] * val[0, 3] * val[3, 1] +
val[3, 0] * val[0, 1] * val[1, 3] -
val[3, 0] * val[0, 3] * val[1, 1];
B[3, 2] = -val[0, 0] * val[1, 1] * val[3, 2] +
val[0, 0] * val[1, 2] * val[3, 1] +
val[1, 0] * val[0, 1] * val[3, 2] -
val[1, 0] * val[0, 2] * val[3, 1] -
val[3, 0] * val[0, 1] * val[1, 2] +
val[3, 0] * val[0, 2] * val[1, 1];
B[0, 3] = -val[0, 1] * val[1, 2] * val[2, 3] +
val[0, 1] * val[1, 3] * val[2, 2] +
val[1, 1] * val[0, 2] * val[2, 3] -
val[1, 1] * val[0, 3] * val[2, 2] -
val[2, 1] * val[0, 2] * val[1, 3] +
val[2, 1] * val[0, 3] * val[1, 2];
B[1, 3] = val[0, 0] * val[1, 2] * val[2, 3] -
val[0, 0] * val[1, 3] * val[2, 2] -
val[1, 0] * val[0, 2] * val[2, 3] +
val[1, 0] * val[0, 3] * val[2, 2] +
val[2, 0] * val[0, 2] * val[1, 3] -
val[2, 0] * val[0, 3] * val[1, 2];
B[2, 3] = -val[0, 0] * val[1, 1] * val[2, 3] +
val[0, 0] * val[1, 3] * val[2, 1] +
val[1, 0] * val[0, 1] * val[2, 3] -
val[1, 0] * val[0, 3] * val[2, 1] -
val[2, 0] * val[0, 1] * val[1, 3] +
val[2, 0] * val[0, 3] * val[1, 1];
B[3, 3] = val[0, 0] * val[1, 1] * val[2, 2] -
val[0, 0] * val[1, 2] * val[2, 1] -
val[1, 0] * val[0, 1] * val[2, 2] +
val[1, 0] * val[0, 2] * val[2, 1] +
val[2, 0] * val[0, 1] * val[1, 2] -
val[2, 0] * val[0, 2] * val[1, 1];
#endregion
det = val[0, 0] * B[0, 0] + val[0, 1] * B[1, 0] + val[0, 2] * B[2, 0] + val[0, 3] * B[3, 0];
//Console.WriteLine("Determinant : {0}", det);
if (det == 0)
{
throw new Exception("Matrix is singular");
}
double det_ = 1.0 / det;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
B[i, j] = B[i, j] * det_;
}
}
return(B);
}
