📄 mgcmatrix3.cpp
字号:
}
}
}
}
//----------------------------------------------------------------------------
void MgcMatrix3::GolubKahanStep (MgcMatrix3& kA, MgcMatrix3& kL,
MgcMatrix3& kR)
{
MgcReal fT11 = kA[0][1]*kA[0][1]+kA[1][1]*kA[1][1];
MgcReal fT22 = kA[1][2]*kA[1][2]+kA[2][2]*kA[2][2];
MgcReal fT12 = kA[1][1]*kA[1][2];
MgcReal fTrace = fT11+fT22;
MgcReal fDiff = fT11-fT22;
MgcReal fDiscr = MgcMath::Sqrt(fDiff*fDiff+4.0*fT12*fT12);
MgcReal fRoot1 = 0.5*(fTrace+fDiscr);
MgcReal fRoot2 = 0.5*(fTrace-fDiscr);
// adjust right
MgcReal fY = kA[0][0] - (MgcMath::Abs(fRoot1-fT22) <=
MgcMath::Abs(fRoot2-fT22) ? fRoot1 : fRoot2);
MgcReal fZ = kA[0][1];
MgcReal fInvLength = 1.0/MgcMath::Sqrt(fY*fY+fZ*fZ);
MgcReal fSin = fZ*fInvLength;
MgcReal fCos = -fY*fInvLength;
MgcReal fTmp0 = kA[0][0];
MgcReal fTmp1 = kA[0][1];
kA[0][0] = fCos*fTmp0-fSin*fTmp1;
kA[0][1] = fSin*fTmp0+fCos*fTmp1;
kA[1][0] = -fSin*kA[1][1];
kA[1][1] *= fCos;
int iRow;
for (iRow = 0; iRow < 3; iRow++)
{
fTmp0 = kR[0][iRow];
fTmp1 = kR[1][iRow];
kR[0][iRow] = fCos*fTmp0-fSin*fTmp1;
kR[1][iRow] = fSin*fTmp0+fCos*fTmp1;
}
// adjust left
fY = kA[0][0];
fZ = kA[1][0];
fInvLength = 1.0/MgcMath::Sqrt(fY*fY+fZ*fZ);
fSin = fZ*fInvLength;
fCos = -fY*fInvLength;
kA[0][0] = fCos*kA[0][0]-fSin*kA[1][0];
fTmp0 = kA[0][1];
fTmp1 = kA[1][1];
kA[0][1] = fCos*fTmp0-fSin*fTmp1;
kA[1][1] = fSin*fTmp0+fCos*fTmp1;
kA[0][2] = -fSin*kA[1][2];
kA[1][2] *= fCos;
int iCol;
for (iCol = 0; iCol < 3; iCol++)
{
fTmp0 = kL[iCol][0];
fTmp1 = kL[iCol][1];
kL[iCol][0] = fCos*fTmp0-fSin*fTmp1;
kL[iCol][1] = fSin*fTmp0+fCos*fTmp1;
}
// adjust right
fY = kA[0][1];
fZ = kA[0][2];
fInvLength = 1.0/MgcMath::Sqrt(fY*fY+fZ*fZ);
fSin = fZ*fInvLength;
fCos = -fY*fInvLength;
kA[0][1] = fCos*kA[0][1]-fSin*kA[0][2];
fTmp0 = kA[1][1];
fTmp1 = kA[1][2];
kA[1][1] = fCos*fTmp0-fSin*fTmp1;
kA[1][2] = fSin*fTmp0+fCos*fTmp1;
kA[2][1] = -fSin*kA[2][2];
kA[2][2] *= fCos;
for (iRow = 0; iRow < 3; iRow++)
{
fTmp0 = kR[1][iRow];
fTmp1 = kR[2][iRow];
kR[1][iRow] = fCos*fTmp0-fSin*fTmp1;
kR[2][iRow] = fSin*fTmp0+fCos*fTmp1;
}
// adjust left
fY = kA[1][1];
fZ = kA[2][1];
fInvLength = 1.0/MgcMath::Sqrt(fY*fY+fZ*fZ);
fSin = fZ*fInvLength;
fCos = -fY*fInvLength;
kA[1][1] = fCos*kA[1][1]-fSin*kA[2][1];
fTmp0 = kA[1][2];
fTmp1 = kA[2][2];
kA[1][2] = fCos*fTmp0-fSin*fTmp1;
kA[2][2] = fSin*fTmp0+fCos*fTmp1;
for (iCol = 0; iCol < 3; iCol++)
{
fTmp0 = kL[iCol][1];
fTmp1 = kL[iCol][2];
kL[iCol][1] = fCos*fTmp0-fSin*fTmp1;
kL[iCol][2] = fSin*fTmp0+fCos*fTmp1;
}
}
//----------------------------------------------------------------------------
void MgcMatrix3::SingularValueDecomposition (MgcMatrix3& kL, MgcVector3& kS,
MgcMatrix3& kR) const
{
const int iMax = 16;
int iRow, iCol;
MgcMatrix3 kA = *this;
Bidiagonalize(kA,kL,kR);
for (int i = 0; i < ms_iSvdMaxIterations; i++)
{
MgcReal fTmp, fTmp0, fTmp1;
MgcReal fSin0, fCos0, fTan0;
MgcReal fSin1, fCos1, fTan1;
bool bTest1 = (MgcMath::Abs(kA[0][1]) <=
ms_fSvdEpsilon*(MgcMath::Abs(kA[0][0])+MgcMath::Abs(kA[1][1])));
bool bTest2 = (MgcMath::Abs(kA[1][2]) <=
ms_fSvdEpsilon*(MgcMath::Abs(kA[1][1])+MgcMath::Abs(kA[2][2])));
if ( bTest1 )
{
if ( bTest2 )
{
kS[0] = kA[0][0];
kS[1] = kA[1][1];
kS[2] = kA[2][2];
break;
}
else
{
// 2x2 closed form factorization
fTmp = (kA[1][1]*kA[1][1] - kA[2][2]*kA[2][2] +
kA[1][2]*kA[1][2])/(kA[1][2]*kA[2][2]);
fTan0 = 0.5*(fTmp+MgcMath::Sqrt(fTmp*fTmp + 4.0));
fCos0 = 1.0/MgcMath::Sqrt(1.0+fTan0*fTan0);
fSin0 = fTan0*fCos0;
for (iCol = 0; iCol < 3; iCol++)
{
fTmp0 = kL[iCol][1];
fTmp1 = kL[iCol][2];
kL[iCol][1] = fCos0*fTmp0-fSin0*fTmp1;
kL[iCol][2] = fSin0*fTmp0+fCos0*fTmp1;
}
fTan1 = (kA[1][2]-kA[2][2]*fTan0)/kA[1][1];
fCos1 = 1.0/MgcMath::Sqrt(1.0+fTan1*fTan1);
fSin1 = -fTan1*fCos1;
for (iRow = 0; iRow < 3; iRow++)
{
fTmp0 = kR[1][iRow];
fTmp1 = kR[2][iRow];
kR[1][iRow] = fCos1*fTmp0-fSin1*fTmp1;
kR[2][iRow] = fSin1*fTmp0+fCos1*fTmp1;
}
kS[0] = kA[0][0];
kS[1] = fCos0*fCos1*kA[1][1] -
fSin1*(fCos0*kA[1][2]-fSin0*kA[2][2]);
kS[2] = fSin0*fSin1*kA[1][1] +
fCos1*(fSin0*kA[1][2]+fCos0*kA[2][2]);
break;
}
}
else
{
if ( bTest2 )
{
// 2x2 closed form factorization
fTmp = (kA[0][0]*kA[0][0] + kA[1][1]*kA[1][1] -
kA[0][1]*kA[0][1])/(kA[0][1]*kA[1][1]);
fTan0 = 0.5*(-fTmp+MgcMath::Sqrt(fTmp*fTmp + 4.0));
fCos0 = 1.0/MgcMath::Sqrt(1.0+fTan0*fTan0);
fSin0 = fTan0*fCos0;
for (iCol = 0; iCol < 3; iCol++)
{
fTmp0 = kL[iCol][0];
fTmp1 = kL[iCol][1];
kL[iCol][0] = fCos0*fTmp0-fSin0*fTmp1;
kL[iCol][1] = fSin0*fTmp0+fCos0*fTmp1;
}
fTan1 = (kA[0][1]-kA[1][1]*fTan0)/kA[0][0];
fCos1 = 1.0/MgcMath::Sqrt(1.0+fTan1*fTan1);
fSin1 = -fTan1*fCos1;
for (iRow = 0; iRow < 3; iRow++)
{
fTmp0 = kR[0][iRow];
fTmp1 = kR[1][iRow];
kR[0][iRow] = fCos1*fTmp0-fSin1*fTmp1;
kR[1][iRow] = fSin1*fTmp0+fCos1*fTmp1;
}
kS[0] = fCos0*fCos1*kA[0][0] -
fSin1*(fCos0*kA[0][1]-fSin0*kA[1][1]);
kS[1] = fSin0*fSin1*kA[0][0] +
fCos1*(fSin0*kA[0][1]+fCos0*kA[1][1]);
kS[2] = kA[2][2];
break;
}
else
{
GolubKahanStep(kA,kL,kR);
}
}
}
// positize diagonal
for (iRow = 0; iRow < 3; iRow++)
{
if ( kS[iRow] < 0.0 )
{
kS[iRow] = -kS[iRow];
for (iCol = 0; iCol < 3; iCol++)
kR[iRow][iCol] = -kR[iRow][iCol];
}
}
}
//----------------------------------------------------------------------------
void MgcMatrix3::SingularValueComposition (const MgcMatrix3& kL,
const MgcVector3& kS, const MgcMatrix3& kR)
{
int iRow, iCol;
MgcMatrix3 kTmp;
// product S*R
for (iRow = 0; iRow < 3; iRow++)
{
for (iCol = 0; iCol < 3; iCol++)
kTmp[iRow][iCol] = kS[iRow]*kR[iRow][iCol];
}
// product L*S*R
for (iRow = 0; iRow < 3; iRow++)
{
for (iCol = 0; iCol < 3; iCol++)
{
m_aafEntry[iRow][iCol] = 0.0;
for (int iMid = 0; iMid < 3; iMid++)
m_aafEntry[iRow][iCol] += kL[iRow][iMid]*kTmp[iMid][iCol];
}
}
}
//----------------------------------------------------------------------------
void MgcMatrix3::Orthonormalize ()
{
// Algorithm uses Gram-Schmidt orthogonalization. If 'this' matrix is
// M = [m0|m1|m2], then orthonormal output matrix is Q = [q0|q1|q2],
//
// q0 = m0/|m0|
// q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
// q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
//
// where |V| indicates length of vector V and A*B indicates dot
// product of vectors A and B.
// compute q0
MgcReal fInvLength = 1.0/MgcMath::Sqrt(m_aafEntry[0][0]*m_aafEntry[0][0]
+ m_aafEntry[1][0]*m_aafEntry[1][0] +
m_aafEntry[2][0]*m_aafEntry[2][0]);
m_aafEntry[0][0] *= fInvLength;
m_aafEntry[1][0] *= fInvLength;
m_aafEntry[2][0] *= fInvLength;
// compute q1
MgcReal fDot0 =
m_aafEntry[0][0]*m_aafEntry[0][1] +
m_aafEntry[1][0]*m_aafEntry[1][1] +
m_aafEntry[2][0]*m_aafEntry[2][1];
m_aafEntry[0][1] -= fDot0*m_aafEntry[0][0];
m_aafEntry[1][1] -= fDot0*m_aafEntry[1][0];
m_aafEntry[2][1] -= fDot0*m_aafEntry[2][0];
fInvLength = 1.0/MgcMath::Sqrt(m_aafEntry[0][1]*m_aafEntry[0][1] +
m_aafEntry[1][1]*m_aafEntry[1][1] +
m_aafEntry[2][1]*m_aafEntry[2][1]);
m_aafEntry[0][1] *= fInvLength;
m_aafEntry[1][1] *= fInvLength;
m_aafEntry[2][1] *= fInvLength;
// compute q2
MgcReal fDot1 =
m_aafEntry[0][1]*m_aafEntry[0][2] +
m_aafEntry[1][1]*m_aafEntry[1][2] +
m_aafEntry[2][1]*m_aafEntry[2][2];
fDot0 =
m_aafEntry[0][0]*m_aafEntry[0][2] +
m_aafEntry[1][0]*m_aafEntry[1][2] +
m_aafEntry[2][0]*m_aafEntry[2][2];
m_aafEntry[0][2] -= fDot0*m_aafEntry[0][0] + fDot1*m_aafEntry[0][1];
m_aafEntry[1][2] -= fDot0*m_aafEntry[1][0] + fDot1*m_aafEntry[1][1];
m_aafEntry[2][2] -= fDot0*m_aafEntry[2][0] + fDot1*m_aafEntry[2][1];
fInvLength = 1.0/MgcMath::Sqrt(m_aafEntry[0][2]*m_aafEntry[0][2] +
m_aafEntry[1][2]*m_aafEntry[1][2] +
m_aafEntry[2][2]*m_aafEntry[2][2]);
m_aafEntry[0][2] *= fInvLength;
m_aafEntry[1][2] *= fInvLength;
m_aafEntry[2][2] *= fInvLength;
}
//----------------------------------------------------------------------------
void MgcMatrix3::QDUDecomposition (MgcMatrix3& kQ,
MgcVector3& kD, MgcVector3& kU) const
{
// Factor M = QR = QDU where Q is orthogonal, D is diagonal,
// and U is upper triangular with ones on its diagonal. Algorithm uses
// Gram-Schmidt orthogonalization (the QR algorithm).
//
// If M = [ m0 | m1 | m2 ] and Q = [ q0 | q1 | q2 ], then
//
// q0 = m0/|m0|
// q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
// q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
//
// where |V| indicates length of vector V and A*B indicates dot
// product of vectors A and B. The matrix R has entries
//
// r00 = q0*m0 r01 = q0*m1 r02 = q0*m2
// r10 = 0 r11 = q1*m1 r12 = q1*m2
// r20 = 0 r21 = 0 r22 = q2*m2
//
// so D = diag(r00,r11,r22) and U has entries u01 = r01/r00,
// u02 = r02/r00, and u12 = r12/r11.
// Q = rotation
// D = scaling
// U = shear
// D stores the three diagonal entries r00, r11, r22
// U stores the entries U[0] = u01, U[1] = u02, U[2] = u12
// build orthogonal matrix Q
MgcReal fInvLength = 1.0/MgcMath::Sqrt(m_aafEntry[0][0]*m_aafEntry[0][0]
+ m_aafEntry[1][0]*m_aafEntry[1][0] +
m_aafEntry[2][0]*m_aafEntry[2][0]);
kQ[0][0] = m_aafEntry[0][0]*fInvLength;
kQ[1][0] = m_aafEntry[1][0]*fInvLength;
kQ[2][0] = m_aafEntry[2][0]*fInvLength;
MgcReal fDot = kQ[0][0]*m_aafEntry[0][1] + kQ[1][0]*m_aafEntry[1][1] +
kQ[2][0]*m_aafEntry[2][1];
kQ[0][1] = m_aafEntry[0][1]-fDot*kQ[0][0];
kQ[1][1] = m_aafEntry[1][1]-fDot*kQ[1][0];
kQ[2][1] = m_aafEntry[2][1]-fDot*kQ[2][0];
fInvLength = 1.0/MgcMath::Sqrt(kQ[0][1]*kQ[0][1] + kQ[1][1]*kQ[1][1] +
kQ[2][1]*kQ[2][1]);
kQ[0][1] *= fInvLength;
kQ[1][1] *= fInvLength;
kQ[2][1] *= fInvLength;
fDot = kQ[0][0]*m_aafEntry[0][2] + kQ[1][0]*m_aafEntry[1][2] +
kQ[2][0]*m_aafEntry[2][2];
kQ[0][2] = m_aafEntry[0][2]-fDot*kQ[0][0];
kQ[1][2] = m_aafEntry[1][2]-fDot*kQ[1][0];
kQ[2][2] = m_aafEntry[2][2]-fDot*kQ[2][0];
fDot = kQ[0][1]*m_aafEntry[0][2] + kQ[1][1]*m_aafEntry[1][2] +
kQ[2][1]*m_aafEntry[2][2];
kQ[0][2] -= fDot*kQ[0][1];
kQ[1][2] -= fDot*kQ[1][1];
kQ[2][2] -= fDot*kQ[2][1];
fInvLength = 1.0/MgcMath::Sqrt(kQ[0][2]*kQ[0][2] + kQ[1][2]*kQ[1][2] +
kQ[2][2]*kQ[2][2]);
kQ[0][2] *= fInvLength;
kQ[1][2] *= fInvLength;
kQ[2][2] *= fInvLength;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -