Wild Magic Library数值计算源码

// Magic Software, Inc. // http://www.magic-software.com // http://www.wild-magic.com // Copyright (c) 2004. All Rights Reserved // // The Wild Magic Library (WML) source code is supplied under the terms of // the license agreement http://www.magic-software.com/License/WildMagic.pdf // and may not be copied or disclosed except in accordance with the terms of // that agreement. #include "WmlLinearSystem.h" using namespace Wml; //---------------------------------------------------------------------------- template <class Real> Real& LinearSystem<Real>::Tolerance () { return ms_fTolerance; } //---------------------------------------------------------------------------- template <class Real> bool LinearSystem<Real>::Solve2 (const Real aafA[2][2], const Real afB[2], Real afX[2]) { Real fDet = aafA[0][0]*aafA[1][1]-aafA[0][1]*aafA[1][0]; if ( Math<Real>::FAbs(fDet) < ms_fTolerance ) return false; Real fInvDet = ((Real)1.0)/fDet; afX[0] = (aafA[1][1]*afB[0]-aafA[0][1]*afB[1])*fInvDet; afX[1] = (aafA[0][0]*afB[1]-aafA[1][0]*afB[0])*fInvDet; return true; } //---------------------------------------------------------------------------- template <class Real> bool LinearSystem<Real>::Solve3 (const Real aafA[3][3], const Real afB[3], Real afX[3]) { Real aafAInv[3][3]; aafAInv[0][0] = aafA[1][1]*aafA[2][2]-aafA[1][2]*aafA[2][1]; aafAInv[0][1] = aafA[0][2]*aafA[2][1]-aafA[0][1]*aafA[2][2]; aafAInv[0][2] = aafA[0][1]*aafA[1][2]-aafA[0][2]*aafA[1][1]; aafAInv[1][0] = aafA[1][2]*aafA[2][0]-aafA[1][0]*aafA[2][2]; aafAInv[1][1] = aafA[0][0]*aafA[2][2]-aafA[0][2]*aafA[2][0]; aafAInv[1][2] = aafA[0][2]*aafA[1][0]-aafA[0][0]*aafA[1][2]; aafAInv[2][0] = aafA[1][0]*aafA[2][1]-aafA[1][1]*aafA[2][0]; aafAInv[2][1] = aafA[0][1]*aafA[2][0]-aafA[0][0]*aafA[2][1]; aafAInv[2][2] = aafA[0][0]*aafA[1][1]-aafA[0][1]*aafA[1][0]; Real fDet = aafA[0][0]*aafAInv[0][0] + aafA[0][1]*aafAInv[1][0] + aafA[0][2]*aafAInv[2][0]; if ( Math<Real>::FAbs(fDet) < ms_fTolerance ) return false; Real fInvDet = ((Real)1.0)/fDet; for (int iRow = 0; iRow < 3; iRow++) { for (int iCol = 0; iCol < 3; iCol++) aafAInv[iRow][iCol] *= fInvDet; } afX[0] = aafAInv[0][0]*afB[0]+aafAInv[0][1]*afB[1]+aafAInv[0][2]*afB[2]; afX[1] = aafAInv[1][0]*afB[0]+aafAInv[1][1]*afB[1]+aafAInv[1][2]*afB[2]; afX[2] = aafAInv[2][0]*afB[0]+aafAInv[2][1]*afB[1]+aafAInv[2][2]*afB[2]; return true; } //---------------------------------------------------------------------------- template <class Real> 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 = 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(rkInvA[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 ) 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; } } } // 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 = rkInvA[i2][aiRowIndex[i1]]; rkInvA[i2][aiRowIndex[i1]] = rkInvA[i2][aiColIndex[i1]]; rkInvA[i2][aiColIndex[i1]] = fSave; } } } delete[] aiColIndex; delete[] aiRowIndex; delete[] abPivoted; return true; } //---------------------------------------------------------------------------- template <class Real> 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++)