📄 wm4matrix3.inl
字号:
fTmp = m_afEntry[2+3*iRow];
m_afEntry[2+3*iRow] = fSin*m_afEntry[1+3*iRow] + fCos*fTmp;
m_afEntry[1+3*iRow] = fCos*m_afEntry[1+3*iRow] - fSin*fTmp;
}
// Update the tridiagonal matrix.
afDiag[1] = fEValue0;
afDiag[2] = fEValue1;
afSubd[0] = (Real)0.0;
afSubd[1] = (Real)0.0;
return true;
}
fSum = Math<Real>::FAbs(afDiag[1]) + Math<Real>::FAbs(afDiag[2]);
if (Math<Real>::FAbs(afSubd[1]) + fSum == fSum)
{
// The matrix is effectively
// +- -+
// M = | d0 s0 0 |
// | s0 d1 0 |
// | 0 0 d2 |
// +- -+
// Compute the eigenvalues as roots of a quadratic equation.
fSum = afDiag[0] + afDiag[1];
fDiff = afDiag[0] - afDiag[1];
fDiscr = Math<Real>::Sqrt(fDiff*fDiff +
((Real)4.0)*afSubd[0]*afSubd[0]);
fEValue0 = ((Real)0.5)*(fSum - fDiscr);
fEValue1 = ((Real)0.5)*(fSum + fDiscr);
// Compute the Givens rotation.
if (fDiff >= (Real)0.0)
{
fCos = afSubd[0];
fSin = afDiag[0] - fEValue0;
}
else
{
fCos = afDiag[1] - fEValue0;
fSin = afSubd[0];
}
fTmp = Math<Real>::InvSqrt(fCos*fCos + fSin*fSin);
fCos *= fTmp;
fSin *= fTmp;
// Postmultiply the current orthogonal matrix with the Givens
// rotation.
for (iRow = 0; iRow < 3; iRow++)
{
fTmp = m_afEntry[1+3*iRow];
m_afEntry[1+3*iRow] = fSin*m_afEntry[0+3*iRow] + fCos*fTmp;
m_afEntry[0+3*iRow] = fCos*m_afEntry[0+3*iRow] - fSin*fTmp;
}
// Update the tridiagonal matrix.
afDiag[0] = fEValue0;
afDiag[1] = fEValue1;
afSubd[0] = (Real)0.0;
afSubd[1] = (Real)0.0;
return true;
}
// The matrix is
// +- -+
// M = | d0 s0 0 |
// | s0 d1 s1 |
// | 0 s1 d2 |
// +- -+
// Set up the parameters for the first pass of the QL step. The
// value for A is the difference between diagonal term D[2] and the
// implicit shift suggested by Wilkinson.
Real fRatio = (afDiag[1]-afDiag[0])/(((Real)2.0f)*afSubd[0]);
Real fRoot = Math<Real>::Sqrt((Real)1.0 + fRatio*fRatio);
Real fB = afSubd[1];
Real fA = afDiag[2] - afDiag[0];
if (fRatio >= (Real)0.0)
{
fA += afSubd[0]/(fRatio + fRoot);
}
else
{
fA += afSubd[0]/(fRatio - fRoot);
}
// Compute the Givens rotation for the first pass.
if (Math<Real>::FAbs(fB) >= Math<Real>::FAbs(fA))
{
fRatio = fA/fB;
fSin = Math<Real>::InvSqrt((Real)1.0 + fRatio*fRatio);
fCos = fRatio*fSin;
}
else
{
fRatio = fB/fA;
fCos = Math<Real>::InvSqrt((Real)1.0 + fRatio*fRatio);
fSin = fRatio*fCos;
}
// Postmultiply the current orthogonal matrix with the Givens
// rotation.
for (iRow = 0; iRow < 3; iRow++)
{
fTmp = m_afEntry[2+3*iRow];
m_afEntry[2+3*iRow] = fSin*m_afEntry[1+3*iRow]+fCos*fTmp;
m_afEntry[1+3*iRow] = fCos*m_afEntry[1+3*iRow]-fSin*fTmp;
}
// Set up the parameters for the second pass of the QL step. The
// values tmp0 and tmp1 are required to fully update the tridiagonal
// matrix at the end of the second pass.
Real fTmp0 = (afDiag[1] - afDiag[2])*fSin +
((Real)2.0)*afSubd[1]*fCos;
Real fTmp1 = fCos*afSubd[0];
fB = fSin*afSubd[0];
fA = fCos*fTmp0 - afSubd[1];
fTmp0 *= fSin;
// Compute the Givens rotation for the second pass. The subdiagonal
// term S[1] in the tridiagonal matrix is updated at this time.
if (Math<Real>::FAbs(fB) >= Math<Real>::FAbs(fA))
{
fRatio = fA/fB;
fRoot = Math<Real>::Sqrt((Real)1.0 + fRatio*fRatio);
afSubd[1] = fB*fRoot;
fSin = ((Real)1.0)/fRoot;
fCos = fRatio*fSin;
}
else
{
fRatio = fB/fA;
fRoot = Math<Real>::Sqrt((Real)1.0 + fRatio*fRatio);
afSubd[1] = fA*fRoot;
fCos = ((Real)1.0)/fRoot;
fSin = fRatio*fCos;
}
// Postmultiply the current orthogonal matrix with the Givens
// rotation.
for (iRow = 0; iRow < 3; iRow++)
{
fTmp = m_afEntry[1+3*iRow];
m_afEntry[1+3*iRow] = fSin*m_afEntry[0+3*iRow]+fCos*fTmp;
m_afEntry[0+3*iRow] = fCos*m_afEntry[0+3*iRow]-fSin*fTmp;
}
// Complete the update of the tridiagonal matrix.
Real fTmp2 = afDiag[1] - fTmp0;
afDiag[2] += fTmp0;
fTmp0 = (afDiag[0] - fTmp2)*fSin + ((Real)2.0)*fTmp1*fCos;
afSubd[0] = fCos*fTmp0 - fTmp1;
fTmp0 *= fSin;
afDiag[1] = fTmp2 + fTmp0;
afDiag[0] -= fTmp0;
}
return false;
}
//----------------------------------------------------------------------------
template <class Real>
void Matrix3<Real>::Bidiagonalize (Matrix3& rkA, Matrix3& rkL, Matrix3& rkR)
{
Real afV[3], afW[3];
Real fLength, fSign, fT1, fInvT1, fT2;
bool bIdentity;
// map first column to (*,0,0)
fLength = Math<Real>::Sqrt(rkA[0][0]*rkA[0][0] + rkA[1][0]*rkA[1][0] +
rkA[2][0]*rkA[2][0]);
if (fLength > (Real)0.0)
{
fSign = (rkA[0][0] > (Real)0.0 ? (Real)1.0 : -(Real)1.0);
fT1 = rkA[0][0] + fSign*fLength;
fInvT1 = ((Real)1.0)/fT1;
afV[1] = rkA[1][0]*fInvT1;
afV[2] = rkA[2][0]*fInvT1;
fT2 = -((Real)2.0)/(((Real)1.0)+afV[1]*afV[1]+afV[2]*afV[2]);
afW[0] = fT2*(rkA[0][0]+rkA[1][0]*afV[1]+rkA[2][0]*afV[2]);
afW[1] = fT2*(rkA[0][1]+rkA[1][1]*afV[1]+rkA[2][1]*afV[2]);
afW[2] = fT2*(rkA[0][2]+rkA[1][2]*afV[1]+rkA[2][2]*afV[2]);
rkA[0][0] += afW[0];
rkA[0][1] += afW[1];
rkA[0][2] += afW[2];
rkA[1][1] += afV[1]*afW[1];
rkA[1][2] += afV[1]*afW[2];
rkA[2][1] += afV[2]*afW[1];
rkA[2][2] += afV[2]*afW[2];
rkL[0][0] = ((Real)1.0)+fT2;
rkL[0][1] = fT2*afV[1];
rkL[1][0] = rkL[0][1];
rkL[0][2] = fT2*afV[2];
rkL[2][0] = rkL[0][2];
rkL[1][1] = ((Real)1.0)+fT2*afV[1]*afV[1];
rkL[1][2] = fT2*afV[1]*afV[2];
rkL[2][1] = rkL[1][2];
rkL[2][2] = ((Real)1.0)+fT2*afV[2]*afV[2];
bIdentity = false;
}
else
{
rkL = Matrix3::IDENTITY;
bIdentity = true;
}
// map first row to (*,*,0)
fLength = Math<Real>::Sqrt(rkA[0][1]*rkA[0][1]+rkA[0][2]*rkA[0][2]);
if (fLength > (Real)0.0)
{
fSign = (rkA[0][1] > (Real)0.0 ? (Real)1.0 : -(Real)1.0);
fT1 = rkA[0][1] + fSign*fLength;
afV[2] = rkA[0][2]/fT1;
fT2 = -((Real)2.0)/(((Real)1.0)+afV[2]*afV[2]);
afW[0] = fT2*(rkA[0][1]+rkA[0][2]*afV[2]);
afW[1] = fT2*(rkA[1][1]+rkA[1][2]*afV[2]);
afW[2] = fT2*(rkA[2][1]+rkA[2][2]*afV[2]);
rkA[0][1] += afW[0];
rkA[1][1] += afW[1];
rkA[1][2] += afW[1]*afV[2];
rkA[2][1] += afW[2];
rkA[2][2] += afW[2]*afV[2];
rkR[0][0] = (Real)1.0;
rkR[0][1] = (Real)0.0;
rkR[1][0] = (Real)0.0;
rkR[0][2] = (Real)0.0;
rkR[2][0] = (Real)0.0;
rkR[1][1] = ((Real)1.0)+fT2;
rkR[1][2] = fT2*afV[2];
rkR[2][1] = rkR[1][2];
rkR[2][2] = ((Real)1.0)+fT2*afV[2]*afV[2];
}
else
{
rkR = Matrix3::IDENTITY;
}
// map second column to (*,*,0)
fLength = Math<Real>::Sqrt(rkA[1][1]*rkA[1][1]+rkA[2][1]*rkA[2][1]);
if (fLength > (Real)0.0)
{
fSign = (rkA[1][1] > (Real)0.0 ? (Real)1.0 : -(Real)1.0);
fT1 = rkA[1][1] + fSign*fLength;
afV[2] = rkA[2][1]/fT1;
fT2 = -((Real)2.0)/(((Real)1.0)+afV[2]*afV[2]);
afW[1] = fT2*(rkA[1][1]+rkA[2][1]*afV[2]);
afW[2] = fT2*(rkA[1][2]+rkA[2][2]*afV[2]);
rkA[1][1] += afW[1];
rkA[1][2] += afW[2];
rkA[2][2] += afV[2]*afW[2];
Real fA = ((Real)1.0)+fT2;
Real fB = fT2*afV[2];
Real fC = ((Real)1.0)+fB*afV[2];
if (bIdentity)
{
rkL[0][0] = (Real)1.0;
rkL[0][1] = (Real)0.0;
rkL[1][0] = (Real)0.0;
rkL[0][2] = (Real)0.0;
rkL[2][0] = (Real)0.0;
rkL[1][1] = fA;
rkL[1][2] = fB;
rkL[2][1] = fB;
rkL[2][2] = fC;
}
else
{
for (int iRow = 0; iRow < 3; iRow++)
{
Real fTmp0 = rkL[iRow][1];
Real fTmp1 = rkL[iRow][2];
rkL[iRow][1] = fA*fTmp0+fB*fTmp1;
rkL[iRow][2] = fB*fTmp0+fC*fTmp1;
}
}
}
}
//----------------------------------------------------------------------------
template <class Real>
void Matrix3<Real>::GolubKahanStep (Matrix3& rkA, Matrix3& rkL, Matrix3& rkR)
{
Real fT11 = rkA[0][1]*rkA[0][1]+rkA[1][1]*rkA[1][1];
Real fT22 = rkA[1][2]*rkA[1][2]+rkA[2][2]*rkA[2][2];
Real fT12 = rkA[1][1]*rkA[1][2];
Real fTrace = fT11+fT22;
Real fDiff = fT11-fT22;
Real fDiscr = Math<Real>::Sqrt(fDiff*fDiff+((Real)4.0)*fT12*fT12);
Real fRoot1 = ((Real)0.5)*(fTrace+fDiscr);
Real fRoot2 = ((Real)0.5)*(fTrace-fDiscr);
// adjust right
Real fY = rkA[0][0] - (Math<Real>::FAbs(fRoot1-fT22) <=
Math<Real>::FAbs(fRoot2-fT22) ? fRoot1 : fRoot2);
Real fZ = rkA[0][1];
Real fInvLength = Math<Real>::InvSqrt(fY*fY+fZ*fZ);
Real fSin = fZ*fInvLength;
Real fCos = -fY*fInvLength;
Real fTmp0 = rkA[0][0];
Real fTmp1 = rkA[0][1];
rkA[0][0] = fCos*fTmp0-fSin*fTmp1;
rkA[0][1] = fSin*fTmp0+fCos*fTmp1;
rkA[1][0] = -fSin*rkA[1][1];
rkA[1][1] *= fCos;
int iRow;
for (iRow = 0; iRow < 3; iRow++)
{
fTmp0 = rkR[0][iRow];
fTmp1 = rkR[1][iRow];
rkR[0][iRow] = fCos*fTmp0-fSin*fTmp1;
rkR[1][iRow] = fSin*fTmp0+fCos*fTmp1;
}
// adjust left
fY = rkA[0][0];
fZ = rkA[1][0];
fInvLength = Math<Real>::InvSqrt(fY*fY+fZ*fZ);
fSin = fZ*fInvLength;
fCos = -fY*fInvLength;
rkA[0][0] = fCos*rkA[0][0]-fSin*rkA[1][0];
fTmp0 = rkA[0][1];
fTmp1 = rkA[1][1];
rkA[0][1] = fCos*fTmp0-fSin*fTmp1;
rkA[1][1] = fSin*fTmp0+fCos*fTmp1;
rkA[0][2] = -fSin*rkA[1][2];
rkA[1][2] *= fCos;
int iCol;
for (iCol = 0; iCol < 3; iCol++)
{
fTmp0 = rkL[iCol][0];
fTmp1 = rkL[iCol][1];
rkL[iCol][0] = fCos*fTmp0-fSin*fTmp1;
rkL[iCol][1] = fSin*fTmp0+fCos*fTmp1;
}
// adjust right
fY = rkA[0][1];
fZ = rkA[0][2];
fInvLength = Math<Real>::InvSqrt(fY*fY+fZ*fZ);
fSin = fZ*fInvLength;
fCos = -fY*fInvLength;
rkA[0][1] = fCos*rkA[0][1]-fSin*rkA[0][2];
fTmp0 = rkA[1][1];
fTmp1 = rkA[1][2];
rkA[1][1] = fCos*fTmp0-fSin*fTmp1;
rkA[1][2] = fSin*fTmp0+fCos*fTmp1;
rkA[2][1] = -fSin*rkA[2][2];
rkA[2][2] *= fCos;
for (iRow = 0; iRow < 3; iRow++)
{
fTmp0 = rkR[1][iRow];
fTmp1 = rkR[2][iRow];
rkR[1][iRow] = fCos*fTmp0-fSin*fTmp1;
rkR[2][iRow] = fSin*fTmp0+fCos*fTmp1;
}
// adjust left
fY = rkA[1][1];
fZ = rkA[2][1];
fInvLength = Math<Real>::InvSqrt(fY*fY+fZ*fZ);
fSin = fZ*fInvLength;
fCos = -fY*fInvLength;
rkA[1][1] = fCos*rkA[1][1]-fSin*rkA[2][1];
fTmp0 = rkA[1][2];
fTmp1 = rkA[2][2];
rkA[1][2] = fCos*fTmp0-fSin*fTmp1;
rkA[2][2] = fSin*fTmp0+fCos*fTmp1;
for (iCol = 0; iCol < 3; iCol++)
{
fTmp0 = rkL[iCol][1];
fTmp1 = rkL[iCol][2];
rkL[iCol][1] = fCos*fTmp0-fSin*fTmp1;
rkL[iCol][2] = fSin*fTmp0+fCos*fTmp1;
}
}
//----------------------------------------------------------------------------
template <class Real>
void Matrix3<Real>::SingularValueDecomposi⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -