float determinant_impl(
float a00, float a01, float a02,
float a10, float a11, float a12,
float a20, float a21, float a22)
{
return a00 * determinant_impl(a11, a12,
a21, a22)
- a01 * determinant_impl(a10, a12,
a20, a22)
+ a02 * determinant_impl(a10, a11,
a20, a21);
}
float mat44::determinant() const
{
return determinant_impl(
_00, _01, _02, _03,
_10, _11, _12, _13,
_20, _21, _22, _23,
_30, _31, _32, _33);
}
float determinant_impl(
float a00, float a01, float a02, float a03,
float a10, float a11, float a12, float a13,
float a20, float a21, float a22, float a23,
float a30, float a31, float a32, float a33)
{
return a00 * determinant_impl(a11, a12, a13,
a21, a22, a23,
a31, a32, a33)
- a01 * determinant_impl(a10, a12, a13,
a20, a22, a23,
a30, a32, a33)
+ a02 * determinant_impl(a10, a11, a13,
a20, a21, a23,
a30, a31, a33)
- a03 * determinant_impl(a10, a11, a12,
a20, a21, a22,
a30, a31, a32);
}
BOOST_QVM_INLINE_OPERATIONS
typename deduce_mat<A>::type
cofactor_impl( A const & a )
{
BOOST_QVM_STATIC_ASSERT(mat_traits<A>::rows==mat_traits<A>::cols);
int const N=mat_traits<A>::rows;
typedef typename mat_traits<A>::scalar_type T;
T c[N-1][N-1];
typedef typename deduce_mat<A>::type R;
R b;
for( int j=0; j!=N; ++j )
{
for( int i=0; i!=N; ++i )
{
int i1=0;
for( int ii=0; ii!=N; ++ii )
{
if( ii==i )
continue;
int j1=0;
for( int jj=0; jj!=N; ++jj )
{
if( jj==j )
continue;
c[i1][j1] = mat_traits<A>::read_element_idx(ii,jj,a);
++j1;
}
++i1;
}
T det = determinant_impl(c);
if( (i+j)&1 )
det=-det;
mat_traits<R>::write_element_idx(i,j,b) = det;
}
}
return b;
}
bool mat44::invert()
{
if (is_orthogonal())
{
transpose();
return true;
}
float det = determinant();
if (!fequal(det, 0.0f))
{
det = 1.0f / det;
mat44 copy(*this);
//to-do det_impl is calculated above in determinant().
//try to gcd
m[0][0] = +determinant_impl(copy._11, copy._12, copy._13, copy._21, copy._22, copy._23, copy._31, copy._32, copy._33) * det;
m[1][0] = -determinant_impl(copy._10, copy._12, copy._13, copy._20, copy._22, copy._23, copy._30, copy._32, copy._33) * det;
m[2][0] = +determinant_impl(copy._10, copy._11, copy._13, copy._20, copy._21, copy._23, copy._30, copy._31, copy._33) * det;
m[3][0] = -determinant_impl(copy._10, copy._11, copy._12, copy._20, copy._21, copy._22, copy._30, copy._31, copy._32) * det;
m[0][1] = -determinant_impl(copy._01, copy._02, copy._03, copy._21, copy._22, copy._23, copy._31, copy._32, copy._33) * det;
m[1][1] = +determinant_impl(copy._00, copy._02, copy._03, copy._20, copy._22, copy._23, copy._30, copy._32, copy._33) * det;
m[2][1] = -determinant_impl(copy._00, copy._01, copy._03, copy._20, copy._21, copy._23, copy._30, copy._31, copy._33) * det;
m[3][1] = +determinant_impl(copy._00, copy._01, copy._02, copy._20, copy._21, copy._22, copy._30, copy._31, copy._32) * det;
m[0][2] = +determinant_impl(copy._01, copy._02, copy._03, copy._11, copy._12, copy._13, copy._31, copy._32, copy._33) * det;
m[1][2] = -determinant_impl(copy._00, copy._02, copy._03, copy._10, copy._12, copy._13, copy._30, copy._32, copy._33) * det;
m[2][2] = +determinant_impl(copy._00, copy._01, copy._03, copy._10, copy._11, copy._13, copy._30, copy._31, copy._33) * det;
m[3][2] = -determinant_impl(copy._00, copy._01, copy._02, copy._10, copy._11, copy._12, copy._30, copy._31, copy._32) * det;
m[0][3] = -determinant_impl(copy._01, copy._02, copy._03, copy._11, copy._12, copy._13, copy._21, copy._22, copy._23) * det;
m[1][3] = +determinant_impl(copy._00, copy._02, copy._03, copy._10, copy._12, copy._13, copy._20, copy._22, copy._23) * det;
m[2][3] = -determinant_impl(copy._00, copy._01, copy._03, copy._10, copy._11, copy._13, copy._20, copy._21, copy._23) * det;
m[3][3] = +determinant_impl(copy._00, copy._01, copy._02, copy._10, copy._11, copy._12, copy._20, copy._21, copy._22) * det;
return true;
}
return false;
}
float mat22::determinant() const
{
return determinant_impl(
_00, _01,
_10, _11);
}
float det33_t(const gml::vec3& row1, const gml::vec3& row2, const gml::vec3& row3)
{
return determinant_impl(row1.x, row2.x, row3.x, row1.y, row2.y, row3.y, row1.z, row2.z, row3.z);
}
