M invert(const ArithSqMat3x3Float<V,M>& _a)
{ /*
// From Graphcis Gems IV, Jean-Francois Doue, C++ Vector and Matrix Algebra Routines.
// From the EULA "Using the code is permitted in any program, product, or library, non-commercial
// or commercial. Giving credit is not required, though is a nice gesture"
M a(_a[0], _a[1], _a[2]);
M b;
b.identity();
unsigned int i, j, i1;
for (j=0; j<3; j++)
{
i1 = j; // Row with largest pivot candidate
for (i=j+1; i<3; i++)
if (fabs(a[i][j]) > fabs(a[i1][j]))
i1 = i;
// Swap rows i1 and j in a and b to put pivot on diagonal
V a_tmp = a[i1];
a[i1] = a[j];
a[j] = a_tmp;
V b_tmp = b[i1];
b[i1] = b[j];
b[j] = b_tmp;
// Scale row j to have a unit diagonal
if (a[j][j] == 0.0)
throw(Mat3x3fSingular("Tried to invert Singular matrix"));
b[j] /= a[j][j];
a[j] /= a[j][j];
// Eliminate off-diagonal elems in col j of a, doing identical ops to b
for (i=0; i<3; i++)
if (i!=j)
{
b[i] -= a[i][j] * b[j];
a[i] -= a[i][j] * a[j];
}
}
return b; */
M a = transpose(_a);
V a1a2 = cross(a[1], a[2]);
float det = dot(a[0], a1a2);
if(fabs(det) < 1.0e-15f)
throw(Mat3x3fSingular("Tried to invert singular matrix"));
return M(a1a2, cross(a[2], a[0]), cross(a[0], a[1]))/det;
}
Mat3x3f invert(const Mat3x3f& _a)
{
Mat3x3f a = _a;
Mat3x3f b = identity_Mat3x3f();
int i, j, i1;
for (j=0; j<3; j++)
{
i1 = j; // Row with largest pivot candidate
for (i=j+1; i<3; i++)
if (fabs(a[i][j]) > fabs(a[i1][j]))
i1 = i;
// Swap rows i1 and j in a and b to put pivot on diagonal
Vec3f a_tmp = a[i1];
a[i1] = a[j];
a[j] = a_tmp;
Vec3f b_tmp = b[i1];
b[i1] = b[j];
b[j] = b_tmp;
// Scale row j to have a unit diagonal
if (a[j][j] == 0.0f)
throw(Mat3x3fSingular("Tried to invert Singular matrix"));
b[j] /= a[j][j];
a[j] /= a[j][j];
// Eliminate off-diagonal elems in col j of a, doing identical ops to b
for (i=0; i<3; i++)
if (i!=j)
{
b[i] -= a[i][j] * b[j];
a[i] -= a[i][j] * a[j];
}
}
return b;
}
