bool LinearSystem<Real>::Inverse (const GMatrix<Real>& A,
GMatrix<Real>& invA)
{
// Computations are performed in-place.
assertion(A.GetNumRows() == A.GetNumColumns(), "Matrix must be square\n");
int size = invA.GetNumRows();
invA = A;
int* colIndex = new1<int>(size);
int* rowIndex = new1<int>(size);
bool* pivoted = new1<bool>(size);
memset(pivoted, 0, size*sizeof(bool));
int i1, i2, row = 0, col = 0;
Real save;
// Elimination by full pivoting.
for (int i0 = 0; i0 < size; ++i0)
{
// Search matrix (excluding pivoted rows) for maximum absolute entry.
Real maxValue = (Real)0;
for (i1 = 0; i1 < size; ++i1)
{
if (!pivoted[i1])
{
for (i2 = 0; i2 < size; ++i2)
{
if (!pivoted[i2])
{
Real absValue = Math<Real>::FAbs(invA[i1][i2]);
if (absValue > maxValue)
{
maxValue = absValue;
row = i1;
col = i2;
}
}
}
}
}
if (maxValue == (Real)0)
{
// The matrix is not invertible.
delete1(colIndex);
delete1(rowIndex);
delete1(pivoted);
return false;
}
pivoted[col] = true;
// Swap rows so that A[col][col] contains the pivot entry.
if (row != col)
{
invA.SwapRows(row,col);
}
// Keep track of the permutations of the rows.
rowIndex[i0] = row;
colIndex[i0] = col;
// Scale the row so that the pivot entry is 1.
Real inv = ((Real)1)/invA[col][col];
invA[col][col] = (Real)1;
for (i2 = 0; i2 < size; i2++)
{
invA[col][i2] *= inv;
}
// Zero out the pivot column locations in the other rows.
for (i1 = 0; i1 < size; ++i1)
{
if (i1 != col)
{
save = invA[i1][col];
invA[i1][col] = (Real)0;
for (i2 = 0; i2 < size; ++i2)
{
invA[i1][i2] -= invA[col][i2]*save;
}
}
}
}
// Reorder rows so that A[][] stores the inverse of the original matrix.
for (i1 = size-1; i1 >= 0; --i1)
{
if (rowIndex[i1] != colIndex[i1])
{
for (i2 = 0; i2 < size; ++i2)
{
save = invA[i2][rowIndex[i1]];
invA[i2][rowIndex[i1]] = invA[i2][colIndex[i1]];
invA[i2][colIndex[i1]] = save;
}
}
}
delete1(colIndex);
delete1(rowIndex);
delete1(pivoted);
return true;
}
bool LinearSystem<Real>::Inverse (const GMatrix<Real>& rkA,
GMatrix<Real>& rkInvA)
{
// computations are performed in-place
assert(rkA.GetRows() == rkA.GetColumns());
int iSize = rkInvA.GetRows();
rkInvA = rkA;
int* aiColIndex = WM4_NEW int[iSize];
int* aiRowIndex = WM4_NEW int[iSize];
bool* abPivoted = WM4_NEW bool[iSize];
memset(abPivoted,0,iSize*sizeof(bool));
int i1, i2, iRow = 0, iCol = 0;
Real fSave;
// elimination by full pivoting
for (int i0 = 0; i0 < iSize; i0++)
{
// search matrix (excluding pivoted rows) for maximum absolute entry
Real fMax = 0.0f;
for (i1 = 0; i1 < iSize; i1++)
{
if (!abPivoted[i1])
{
for (i2 = 0; i2 < iSize; i2++)
{
if (!abPivoted[i2])
{
Real fAbs = Math<Real>::FAbs(rkInvA[i1][i2]);
if (fAbs > fMax)
{
fMax = fAbs;
iRow = i1;
iCol = i2;
}
}
}
}
}
if (fMax == (Real)0.0)
{
// matrix is not invertible
WM4_DELETE[] aiColIndex;
WM4_DELETE[] aiRowIndex;
WM4_DELETE[] abPivoted;
return false;
}
abPivoted[iCol] = true;
// swap rows so that A[iCol][iCol] contains the pivot entry
if (iRow != iCol)
{
rkInvA.SwapRows(iRow,iCol);
}
// keep track of the permutations of the rows
aiRowIndex[i0] = iRow;
aiColIndex[i0] = iCol;
// scale the row so that the pivot entry is 1
Real fInv = ((Real)1.0)/rkInvA[iCol][iCol];
rkInvA[iCol][iCol] = (Real)1.0;
for (i2 = 0; i2 < iSize; i2++)
{
rkInvA[iCol][i2] *= fInv;
}
// zero out the pivot column locations in the other rows
for (i1 = 0; i1 < iSize; i1++)
{
if (i1 != iCol)
{
fSave = rkInvA[i1][iCol];
rkInvA[i1][iCol] = (Real)0.0;
for (i2 = 0; i2 < iSize; i2++)
{
rkInvA[i1][i2] -= rkInvA[iCol][i2]*fSave;
}
}
}
}
bool LinearSystem<Real>::Solve (const GMatrix<Real>& rkA, const Real* afB,
Real* afX)
{
// computations are performed in-place
int iSize = rkA.GetColumns();
GMatrix<Real> kInvA = rkA;
memcpy(afX,afB,iSize*sizeof(Real));
int* aiColIndex = new int[iSize];
assert( aiColIndex );
int* aiRowIndex = new int[iSize];
assert( aiRowIndex );
bool* abPivoted = new bool[iSize];
assert( abPivoted );
memset(abPivoted,0,iSize*sizeof(bool));
int i1, i2, iRow = 0, iCol = 0;
Real fSave;
// elimination by full pivoting
for (int i0 = 0; i0 < iSize; i0++)
{
// search matrix (excluding pivoted rows) for maximum absolute entry
Real fMax = 0.0f;
for (i1 = 0; i1 < iSize; i1++)
{
if ( !abPivoted[i1] )
{
for (i2 = 0; i2 < iSize; i2++)
{
if ( !abPivoted[i2] )
{
Real fAbs = Math<Real>::FAbs(kInvA[i1][i2]);
if ( fAbs > fMax )
{
fMax = fAbs;
iRow = i1;
iCol = i2;
}
}
}
}
}
if ( fMax == (Real)0.0 )
{
// matrix is not invertible
delete[] aiColIndex;
delete[] aiRowIndex;
delete[] abPivoted;
return false;
}
abPivoted[iCol] = true;
// swap rows so that A[iCol][iCol] contains the pivot entry
if ( iRow != iCol )
{
kInvA.SwapRows(iRow,iCol);
fSave = afX[iRow];
afX[iRow] = afX[iCol];
afX[iCol] = fSave;
}
// keep track of the permutations of the rows
aiRowIndex[i0] = iRow;
aiColIndex[i0] = iCol;
// scale the row so that the pivot entry is 1
Real fInv = ((Real)1.0)/kInvA[iCol][iCol];
kInvA[iCol][iCol] = (Real)1.0;
for (i2 = 0; i2 < iSize; i2++)
kInvA[iCol][i2] *= fInv;
afX[iCol] *= fInv;
// zero out the pivot column locations in the other rows
for (i1 = 0; i1 < iSize; i1++)
{
if ( i1 != iCol )
{
fSave = kInvA[i1][iCol];
kInvA[i1][iCol] = (Real)0.0;
for (i2 = 0; i2 < iSize; i2++)
kInvA[i1][i2] -= kInvA[iCol][i2]*fSave;
afX[i1] -= afX[iCol]*fSave;
}
}
}
// reorder rows so that A[][] stores the inverse of the original matrix
for (i1 = iSize-1; i1 >= 0; i1--)
{
if ( aiRowIndex[i1] != aiColIndex[i1] )
{
for (i2 = 0; i2 < iSize; i2++)
{
fSave = kInvA[i2][aiRowIndex[i1]];
kInvA[i2][aiRowIndex[i1]] = kInvA[i2][aiColIndex[i1]];
kInvA[i2][aiColIndex[i1]] = fSave;
}
}
}
delete[] aiColIndex;
delete[] aiRowIndex;
delete[] abPivoted;
return true;
}
