void Matrix4D::Invert()
{
Matrix4D inverse;
inverse.Identity();
if ( Math::fabs( m[0][0] ) < Math::fabs( m[1][0] ) ) Swap_Rows( 0, 1 );
Scalar one_over = 1.0f / m[0][0];
Scalar scale_factor = m[1][0];
// generate row echelon form
for ( int j = 0; j < dimension; j++ )
{
m[0][j] *= one_over;
inverse.m[0][j] *= one_over;
m[1][j] -= m[0][j] * scale_factor;
inverse.m[1][j] -= inverse.m[0][j] * scale_factor;
}
// back substitution
one_over = 1.0f / m[1][1];
scale_factor = m[0][1];
for ( int j = dimension-1; j >= 0; j-- )
{
m[1][j] *= one_over;
inverse.m[1][j] *= one_over;
m[0][j] -= m[1][j] * scale_factor;
inverse.m[0][j] -= inverse.m[1][j] * scale_factor;
}
*this = inverse;
}
// Perform Gaussian elimination with back substituion, given vector b,
// find vector X such that Ax = b
XVECTOR XSQUARE_MATRIX::Solve_GEb(const XVECTOR &i_vB)
{
XVECTOR vX;
XVECTOR vBlcl = i_vB;
if (m_lpdValues && i_vB.m_lpdValues && i_vB.m_uiN == m_uiN)
{
double * lpdValues_Store = new double [m_uiN * m_uiN];
// store original matrix
memcpy(lpdValues_Store,m_lpdValues,
sizeof(double) * m_uiN * m_uiN);
vX.Set_Size(m_uiN);
unsigned int uiRef_Idx = 0;
double dMax = 0.0;
unsigned int uiRow, uiRowInner, uiCol;
for (uiRow = 0; uiRow < m_uiN; uiRow++)
{
// find max valued row
uiRef_Idx = uiRow;
for (uiRowInner = uiRow; uiRowInner < m_uiN; uiRowInner++)
{
if (m_lpdValues[uiRowInner * m_uiN + uiRow] > dMax)
{
uiRef_Idx = uiRowInner;
dMax = m_lpdValues[uiRowInner * m_uiN + uiRow];
}
}
Swap_Rows(uiRow,uiRef_Idx);
vBlcl.Swap_Rows(uiRow,uiRef_Idx);
// Perform Gaussian elimination for this column
for (uiRowInner = uiRow + 1; uiRowInner < m_uiN; uiRowInner++)
{
double dInner_Scalar =
-m_lpdValues[uiRowInner * m_uiN + uiRow] /
m_lpdValues[uiRow * m_uiN + uiRow];
for (uiCol = uiRow; uiCol < m_uiN; uiCol++)
{
m_lpdValues[uiRowInner * m_uiN + uiCol] +=
m_lpdValues[uiRow * m_uiN + uiCol] *
dInner_Scalar;
}
vBlcl.m_lpdValues[uiRowInner] +=
vBlcl.m_lpdValues[uiRow] * dInner_Scalar;
}
}
// Perform backsubstituion step
vX = Back_Substituion(m_lpdValues,vBlcl);
// restore original matrix
memcpy(m_lpdValues,lpdValues_Store,
sizeof(double) * m_uiN * m_uiN);
delete [] lpdValues_Store;
}
return vX;
}
// perform Guass-Jordanian elimination: The matrix will become
// it's inverse
void XSQUARE_MATRIX::Inverse_GJ(void)
{
if (m_lpdValues)
{
XSQUARE_MATRIX cI(m_uiN);
cI.Identity();
// allocate work space
double dScalar;
// Using row reduction, reduce the source matrix to the identity,
// and the identity will become A^{-1}
for (unsigned int uiRow = 0; uiRow < m_uiN; uiRow++)
{
if (m_lpdValues[uiRow * m_uiN + uiRow] == 0.0)
{
for (unsigned int uiRowInner = uiRow + 1;
uiRowInner < m_uiN;
uiRowInner++)
{
if (m_lpdValues[uiRowInner * m_uiN + uiRow] != 0.0)
{
Swap_Rows(uiRow,uiRowInner);
cI.Swap_Rows(uiRow,uiRowInner);
}
}
}
dScalar = 1.0 / m_lpdValues[uiRow * m_uiN + uiRow];
Scale_Row(uiRow,dScalar);
cI.Scale_Row(uiRow,dScalar);
for (unsigned int uiRowInner = 0;
uiRowInner < m_uiN;
uiRowInner++)
{
double dRow_Scalar =
-m_lpdValues[uiRowInner * m_uiN + uiRow];
if (uiRowInner != uiRow)
{
cI.Add_Rows(uiRowInner,uiRow,dRow_Scalar,false);
Add_Rows(uiRowInner,uiRow,dRow_Scalar,true);
}
}
}
// copy result into current matrix
*this = cI;
}
}
