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; }