Eigen<Real>::Eigen (const GMatrix<Real>& rkM)
:
m_kMat(rkM)
{
m_iSize = rkM.GetRows();
assert(m_iSize >= 2 && (rkM.GetColumns() == m_iSize));
m_afDiag = WM4_NEW Real[m_iSize];
m_afSubd = WM4_NEW Real[m_iSize];
m_bIsRotation = false;
}
Eigen<Real>::Eigen (const GMatrix<Real>& rkM)
:
m_kMat(rkM)
{
m_iSize = rkM.GetRows();
assert( m_iSize >= 2 && (rkM.GetColumns() == m_iSize) );
m_afDiag = new Real[m_iSize];
m_afSubd = new Real[m_iSize];
// set according to the parity of the number of Householder reflections
m_bIsRotation = ((m_iSize % 2) == 0);
}
EigenDecomposition<Real>::EigenDecomposition (const GMatrix<Real>& mat)
:
mMatrix(mat)
{
mSize = mat.GetRows();
assertion(mSize >= 2 && (mat.GetColumns() == mSize),
"Square matrix required in EigenDecomposition constructor\n");
mDiagonal = new1<Real>(mSize);
mSubdiagonal = new1<Real>(mSize);
mIsRotation = false;
}
bool LinearSystem<Real>::SolveSymmetricCG (const GMatrix<Real>& rkA,
const Real* afB, Real* afX)
{
// based on the algorithm in "Matrix Computations" by Golum and Van Loan
assert( rkA.GetRows() == rkA.GetColumns() );
int iSize = rkA.GetRows();
Real* afR = new Real[iSize];
Real* afP = new Real[iSize];
Real* afW = new Real[iSize];
// first iteration
memset(afX,0,iSize*sizeof(Real));
memcpy(afR,afB,iSize*sizeof(Real));
Real fRho0 = Dot(iSize,afR,afR);
memcpy(afP,afR,iSize*sizeof(Real));
Multiply(rkA,afP,afW);
Real fAlpha = fRho0/Dot(iSize,afP,afW);
UpdateX(iSize,afX,fAlpha,afP);
UpdateR(iSize,afR,fAlpha,afW);
Real fRho1 = Dot(iSize,afR,afR);
// remaining iterations
const int iMax = 1024;
int i;
for (i = 1; i < iMax; i++)
{
Real fRoot0 = Math<Real>::Sqrt(fRho1);
Real fNorm = Dot(iSize,afB,afB);
Real fRoot1 = Math<Real>::Sqrt(fNorm);
if ( fRoot0 <= ms_fTolerance*fRoot1 )
break;
Real fBeta = fRho1/fRho0;
UpdateP(iSize,afP,fBeta,afR);
Multiply(rkA,afP,afW);
fAlpha = fRho1/Dot(iSize,afP,afW);
UpdateX(iSize,afX,fAlpha,afP);
UpdateR(iSize,afR,fAlpha,afW);
fRho0 = fRho1;
fRho1 = Dot(iSize,afR,afR);
}
delete[] afW;
delete[] afP;
delete[] afR;
return i < iMax;
}
void LinearSystem<Real>::BackwardEliminate (int iReduceRow,
BandedMatrix<Real>& rkA, GMatrix<Real>& rkB)
{
int iRowMax = iReduceRow - 1;
int iRowMin = iReduceRow - rkA.GetUBands();
if ( iRowMin < 0 )
iRowMin = 0;
for (int iRow = iRowMax; iRow >= iRowMin; iRow--)
{
Real fMult = rkA(iRow,iReduceRow);
rkA(iRow,iReduceRow) = (Real)0.0;
for (int iCol = 0; iCol < rkB.GetColumns(); iCol++)
rkB(iRow,iCol) -= fMult*rkB(iReduceRow,iCol);
}
}
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>::SolveSymmetric (const GMatrix<Real>& rkA,
const Real* afB, Real* afX)
{
// A = L D L^t decomposition with diagonal terms of L equal to 1. The
// algorithm stores D terms in A[i][i] and off-diagonal L terms in
// A[i][j] for i > j. (G. Golub and C. Van Loan, Matrix Computations)
// computations are performed in-place
int iSize = rkA.GetColumns();
GMatrix<Real> kLMat = rkA;
memcpy(afX,afB,iSize*sizeof(Real));
int i0, i1;
Real* afV = new Real[iSize];
assert( afV );
for (i1 = 0; i1 < iSize; i1++)
{
for (i0 = 0; i0 < i1; i0++)
afV[i0] = kLMat[i1][i0]*kLMat[i0][i0];
afV[i1] = kLMat[i1][i1];
for (i0 = 0; i0 < i1; i0++)
afV[i1] -= kLMat[i1][i0]*afV[i0];
kLMat[i1][i1] = afV[i1];
if ( Math<Real>::FAbs(afV[i1]) <= Math<Real>::EPSILON )
{
delete[] afV;
return false;
}
Real fInv = ((Real)1.0)/afV[i1];
for (i0 = i1+1; i0 < iSize; i0++)
{
for (int i2 = 0; i2 < i1; i2++)
kLMat[i0][i1] -= kLMat[i0][i2]*afV[i2];
kLMat[i0][i1] *= fInv;
}
}
delete[] afV;
// Solve Ax = B.
// Forward substitution: Let z = DL^t x, then Lz = B. Algorithm
// stores z terms in B vector.
for (i0 = 0; i0 < iSize; i0++)
{
for (i1 = 0; i1 < i0; i1++)
afX[i0] -= kLMat[i0][i1]*afX[i1];
}
// Diagonal division: Let y = L^t x, then Dy = z. Algorithm stores
// y terms in B vector.
for (i0 = 0; i0 < iSize; i0++)
{
if ( Math<Real>::FAbs(kLMat[i0][i0]) <= Math<Real>::EPSILON )
return false;
afX[i0] /= kLMat[i0][i0];
}
// Back substitution: Solve L^t x = y.
for (i0 = iSize-2; i0 >= 0; i0--)
{
for (i1 = i0+1; i1 < iSize; i1++)
afX[i0] -= kLMat[i1][i0]*afX[i1];
}
return true;
}
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;
}
