/** 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;
}
/** Matrix inverse assuming the array follows this index pattern.
* [0 1]
* [2 3]
* @return true if the matrix was inverted. false if no solution.
*/
bool invert2x2matrix(const double *input, double *output)
{
double det = determinant2x2(input[0], input[1], input[2], input[3]);
if(fabs(det) < MY_EPSILON) return false;
output[0] = input[3] / det;
output[1] = -input[1] / det;
output[2] = -input[2] / det;
output[3] = input[0] / 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 double determinant3x3(double a1, double a2, double a3, double b1, double b2, double b3, double c1, double c2, double c3)
{
return a1 * determinant2x2(b2, b3, c2, c3)
- b1 * determinant2x2(a2, a3, c2, c3)
+ c1 * determinant2x2(a2, a3, b2, b3);
}