static double determinant4x4(const TransformationMatrix::Matrix4& m)
{
// Assign to individual variable names to aid selecting
// correct elements
double a1 = m[0][0];
double b1 = m[0][1];
double c1 = m[0][2];
double d1 = m[0][3];
double a2 = m[1][0];
double b2 = m[1][1];
double c2 = m[1][2];
double d2 = m[1][3];
double a3 = m[2][0];
double b3 = m[2][1];
double c3 = m[2][2];
double d3 = m[2][3];
double a4 = m[3][0];
double b4 = m[3][1];
double c4 = m[3][2];
double d4 = m[3][3];
return a1 * determinant3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4)
- b1 * determinant3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4)
+ c1 * determinant3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4)
- d1 * determinant3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
}
static inline double determinant(const Matrix& matrix)
{
const Matrix::MatrixData & m = matrix.m_matrix;
const double a1 = m[0][0];
const double b1 = m[0][1];
const double c1 = m[0][2];
const double d1 = m[0][3];
const double a2 = m[1][0];
const double b2 = m[1][1];
const double c2 = m[1][2];
const double d2 = m[1][3];
const double a3 = m[2][0];
const double b3 = m[2][1];
const double c3 = m[2][2];
const double d3 = m[2][3];
const double a4 = m[3][0];
const double b4 = m[3][1];
const double c4 = m[3][2];
const double d4 = m[3][3];
return a1 * determinant3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4)
- b1 * determinant3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4)
+ c1 * determinant3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4)
- d1 * determinant3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
}
/** Matrix inverse assuming the array follows this index pattern.
* [0 1 2]
* [3 4 5]
* [6 7 8]
* @param x matrix to invert
* @param y stores the result in y
* @return true if the matrix was inverted. false if no solution.
*/
bool invert3x3matrix(const double *x, double *y)
{
double det = determinant3x3(x);
if(fabs(det) < MY_EPSILON) return false;
y[0] = determinant2x2(x[4], x[5], x[7], x[8]) / det;
y[1] = determinant2x2(x[2], x[1], x[8], x[7]) / det;
y[2] = determinant2x2(x[1], x[2], x[4], x[5]) / det;
y[3] = determinant2x2(x[5], x[3], x[8], x[6]) / det;
y[4] = determinant2x2(x[0], x[2], x[6], x[8]) / det;
y[5] = determinant2x2(x[2], x[0], x[5], x[3]) / det;
y[6] = determinant2x2(x[3], x[4], x[6], x[7]) / det;
y[7] = determinant2x2(x[1], x[0], x[7], x[6]) / det;
y[8] = determinant2x2(x[0], x[1], x[3], x[4]) / det;
return true;
}
double determinant(const Matrix& mat)
{
if(mat.getm()==1)
return determinant1x1(mat);
else if(mat.getm()==2)
return determinant2x2(mat);
else if(mat.getm()==3)
return determinant3x3(mat);
double det = 0;
Matrix * subMat[mat.getm()];
for(int i=0; i<mat.getm(); i++)
{
subMat[i] = new Matrix(mat.getm()-1, mat.getm()-1);
}
for(int i=0; i<mat.getm(); i++)
{
for(int j=0; j<mat.getm()-1; j++)
{
for(int k=0, l=0; k<mat.getm()-1; k++,l++)
{
if(i==l)
l++;
(*(subMat[i]))[j][k]=mat[j+1][l];
}
}
}
for(int i=0, sign=1; i<mat.getm(); i++)
{
det+=sign*mat[0][i]*determinant(*subMat[i]);
if(sign==-1)
sign=1;
else
sign=-1;
}
for(int i=0; i<mat.getm(); i++)
{
delete subMat[i];
}
return det;
}
static void adjoint(const TransformationMatrix::Matrix4& matrix, TransformationMatrix::Matrix4& result)
{
// Assign to individual variable names to aid
// selecting correct values
double a1 = matrix[0][0];
double b1 = matrix[0][1];
double c1 = matrix[0][2];
double d1 = matrix[0][3];
double a2 = matrix[1][0];
double b2 = matrix[1][1];
double c2 = matrix[1][2];
double d2 = matrix[1][3];
double a3 = matrix[2][0];
double b3 = matrix[2][1];
double c3 = matrix[2][2];
double d3 = matrix[2][3];
double a4 = matrix[3][0];
double b4 = matrix[3][1];
double c4 = matrix[3][2];
double d4 = matrix[3][3];
// Row column labeling reversed since we transpose rows & columns
result[0][0] = determinant3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4);
result[1][0] = - determinant3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4);
result[2][0] = determinant3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4);
result[3][0] = - determinant3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
result[0][1] = - determinant3x3(b1, b3, b4, c1, c3, c4, d1, d3, d4);
result[1][1] = determinant3x3(a1, a3, a4, c1, c3, c4, d1, d3, d4);
result[2][1] = - determinant3x3(a1, a3, a4, b1, b3, b4, d1, d3, d4);
result[3][1] = determinant3x3(a1, a3, a4, b1, b3, b4, c1, c3, c4);
result[0][2] = determinant3x3(b1, b2, b4, c1, c2, c4, d1, d2, d4);
result[1][2] = - determinant3x3(a1, a2, a4, c1, c2, c4, d1, d2, d4);
result[2][2] = determinant3x3(a1, a2, a4, b1, b2, b4, d1, d2, d4);
result[3][2] = - determinant3x3(a1, a2, a4, b1, b2, b4, c1, c2, c4);
result[0][3] = - determinant3x3(b1, b2, b3, c1, c2, c3, d1, d2, d3);
result[1][3] = determinant3x3(a1, a2, a3, c1, c2, c3, d1, d2, d3);
result[2][3] = - determinant3x3(a1, a2, a3, b1, b2, b3, d1, d2, d3);
result[3][3] = determinant3x3(a1, a2, a3, b1, b2, b3, c1, c2, c3);
}
