📄 wmlmatrix3.cpp
字号:
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>::SingularValueDecomposition (Matrix3& rkL, Matrix3& rkS,
Matrix3& rkR) const
{
int iRow, iCol;
Matrix3 kA = *this;
Bidiagonalize(kA,rkL,rkR);
rkS.MakeZero();
const int iMax = 32;
const Real fEpsilon = (Real)1e-04;
for (int i = 0; i < iMax; i++)
{
Real fTmp, fTmp0, fTmp1;
Real fSin0, fCos0, fTan0;
Real fSin1, fCos1, fTan1;
bool bTest1 = (Math<Real>::FAbs(kA[0][1]) <=
fEpsilon*(Math<Real>::FAbs(kA[0][0]) +
Math<Real>::FAbs(kA[1][1])));
bool bTest2 = (Math<Real>::FAbs(kA[1][2]) <=
fEpsilon*(Math<Real>::FAbs(kA[1][1]) +
Math<Real>::FAbs(kA[2][2])));
if ( bTest1 )
{
if ( bTest2 )
{
rkS[0][0] = kA[0][0];
rkS[1][1] = kA[1][1];
rkS[2][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 = ((Real)0.5)*(fTmp + Math<Real>::Sqrt(fTmp*fTmp +
((Real)4.0)));
fCos0 = Math<Real>::InvSqrt(((Real)1.0)+fTan0*fTan0);
fSin0 = fTan0*fCos0;
for (iCol = 0; iCol < 3; iCol++)
{
fTmp0 = rkL[iCol][1];
fTmp1 = rkL[iCol][2];
rkL[iCol][1] = fCos0*fTmp0-fSin0*fTmp1;
rkL[iCol][2] = fSin0*fTmp0+fCos0*fTmp1;
}
fTan1 = (kA[1][2]-kA[2][2]*fTan0)/kA[1][1];
fCos1 = Math<Real>::InvSqrt(((Real)1.0)+fTan1*fTan1);
fSin1 = -fTan1*fCos1;
for (iRow = 0; iRow < 3; iRow++)
{
fTmp0 = rkR[1][iRow];
fTmp1 = rkR[2][iRow];
rkR[1][iRow] = fCos1*fTmp0-fSin1*fTmp1;
rkR[2][iRow] = fSin1*fTmp0+fCos1*fTmp1;
}
rkS[0][0] = kA[0][0];
rkS[1][1] = fCos0*fCos1*kA[1][1] -
fSin1*(fCos0*kA[1][2]-fSin0*kA[2][2]);
rkS[2][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 = ((Real)0.5)*(-fTmp + Math<Real>::Sqrt(fTmp*fTmp +
((Real)4.0)));
fCos0 = Math<Real>::InvSqrt(((Real)1.0)+fTan0*fTan0);
fSin0 = fTan0*fCos0;
for (iCol = 0; iCol < 3; iCol++)
{
fTmp0 = rkL[iCol][0];
fTmp1 = rkL[iCol][1];
rkL[iCol][0] = fCos0*fTmp0-fSin0*fTmp1;
rkL[iCol][1] = fSin0*fTmp0+fCos0*fTmp1;
}
fTan1 = (kA[0][1]-kA[1][1]*fTan0)/kA[0][0];
fCos1 = Math<Real>::InvSqrt(((Real)1.0)+fTan1*fTan1);
fSin1 = -fTan1*fCos1;
for (iRow = 0; iRow < 3; iRow++)
{
fTmp0 = rkR[0][iRow];
fTmp1 = rkR[1][iRow];
rkR[0][iRow] = fCos1*fTmp0-fSin1*fTmp1;
rkR[1][iRow] = fSin1*fTmp0+fCos1*fTmp1;
}
rkS[0][0] = fCos0*fCos1*kA[0][0] -
fSin1*(fCos0*kA[0][1]-fSin0*kA[1][1]);
rkS[1][1] = fSin0*fSin1*kA[0][0] +
fCos1*(fSin0*kA[0][1]+fCos0*kA[1][1]);
rkS[2][2] = kA[2][2];
break;
}
else
{
GolubKahanStep(kA,rkL,rkR);
}
}
}
// positize diagonal
for (iRow = 0; iRow < 3; iRow++)
{
if ( rkS[iRow][iRow] < (Real)0.0 )
{
rkS[iRow][iRow] = -rkS[iRow][iRow];
for (iCol = 0; iCol < 3; iCol++)
rkR[iRow][iCol] = -rkR[iRow][iCol];
}
}
}
//----------------------------------------------------------------------------
template <class Real>
void Matrix3<Real>::SingularValueComposition (const Matrix3& rkL,
const Matrix3& rkS, const Matrix3& rkR)
{
*this = rkL*(rkS*rkR);
}
//----------------------------------------------------------------------------
template <class Real>
void Matrix3<Real>::QDUDecomposition (Matrix3& rkQ, Matrix3& rkD,
Matrix3& rkU) const
{
// Factor M = QR = QDU where Q is orthogonal (rotation), D is diagonal
// (scaling), and U is upper triangular with ones on its diagonal
// (shear). 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.
// build orthogonal matrix Q
Real fInvLength = Math<Real>::InvSqrt(m_afEntry[0]*m_afEntry[0] +
m_afEntry[3]*m_afEntry[3] + m_afEntry[6]*m_afEntry[6]);
rkQ[0][0] = m_afEntry[0]*fInvLength;
rkQ[1][0] = m_afEntry[3]*fInvLength;
rkQ[2][0] = m_afEntry[6]*fInvLength;
Real fDot = rkQ[0][0]*m_afEntry[1] + rkQ[1][0]*m_afEntry[4] +
rkQ[2][0]*m_afEntry[7];
rkQ[0][1] = m_afEntry[1]-fDot*rkQ[0][0];
rkQ[1][1] = m_afEntry[4]-fDot*rkQ[1][0];
rkQ[2][1] = m_afEntry[7]-fDot*rkQ[2][0];
fInvLength = Math<Real>::InvSqrt(rkQ[0][1]*rkQ[0][1] +
rkQ[1][1]*rkQ[1][1] + rkQ[2][1]*rkQ[2][1]);
rkQ[0][1] *= fInvLength;
rkQ[1][1] *= fInvLength;
rkQ[2][1] *= fInvLength;
fDot = rkQ[0][0]*m_afEntry[2] + rkQ[1][0]*m_afEntry[5] +
rkQ[2][0]*m_afEntry[8];
rkQ[0][2] = m_afEntry[2]-fDot*rkQ[0][0];
rkQ[1][2] = m_afEntry[5]-fDot*rkQ[1][0];
rkQ[2][2] = m_afEntry[8]-fDot*rkQ[2][0];
fDot = rkQ[0][1]*m_afEntry[2] + rkQ[1][1]*m_afEntry[5] +
rkQ[2][1]*m_afEntry[8];
rkQ[0][2] -= fDot*rkQ[0][1];
rkQ[1][2] -= fDot*rkQ[1][1];
rkQ[2][2] -= fDot*rkQ[2][1];
fInvLength = Math<Real>::InvSqrt(rkQ[0][2]*rkQ[0][2] +
rkQ[1][2]*rkQ[1][2] + rkQ[2][2]*rkQ[2][2]);
rkQ[0][2] *= fInvLength;
rkQ[1][2] *= fInvLength;
rkQ[2][2] *= fInvLength;
// guarantee that orthogonal matrix has determinant 1 (no reflections)
Real fDet = rkQ[0][0]*rkQ[1][1]*rkQ[2][2] + rkQ[0][1]*rkQ[1][2]*rkQ[2][0]
+ rkQ[0][2]*rkQ[1][0]*rkQ[2][1] - rkQ[0][2]*rkQ[1][1]*rkQ[2][0]
- rkQ[0][1]*rkQ[1][0]*rkQ[2][2] - rkQ[0][0]*rkQ[1][2]*rkQ[2][1];
if ( fDet < (Real)0.0 )
{
for (int iRow = 0; iRow < 3; iRow++)
{
for (int iCol = 0; iCol < 3; iCol++)
rkQ[iRow][iCol] = -rkQ[iRow][iCol];
}
}
// build "right" matrix R
Matrix3 kR;
kR[0][0] = rkQ[0][0]*m_afEntry[0] + rkQ[1][0]*m_afEntry[3] +
rkQ[2][0]*m_afEntry[6];
kR[0][1] = rkQ[0][0]*m_afEntry[1] + rkQ[1][0]*m_afEntry[4] +
rkQ[2][0]*m_afEntry[7];
kR[1][1] = rkQ[0][1]*m_afEntry[1] + rkQ[1][1]*m_afEntry[4] +
rkQ[2][1]*m_afEntry[7];
kR[0][2] = rkQ[0][0]*m_afEntry[2] + rkQ[1][0]*m_afEntry[5] +
rkQ[2][0]*m_afEntry[8];
kR[1][2] = rkQ[0][1]*m_afEntry[2] + rkQ[1][1]*m_afEntry[5] +
rkQ[2][1]*m_afEntry[8];
kR[2][2] = rkQ[0][2]*m_afEntry[2] + rkQ[1][2]*m_afEntry[5] +
rkQ[2][2]*m_afEntry[8];
// the scaling component
rkD.MakeDiagonal(kR[0][0],kR[1][1],kR[2][2]);
// the shear component
Real fInvD0 = ((Real)1.0)/rkD[0][0];
rkU[0][0] = (Real)1.0;
rkU[0][1] = kR[0][1]*fInvD0;
rkU[0][2] = kR[0][2]*fInvD0;
rkU[1][0] = (Real)0.0;
rkU[1][1] = (Real)1.0;
rkU[1][2] = kR[1][2]/rkD[1][1];
rkU[2][0] = (Real)0.0;
rkU[2][1] = (Real)0.0;
rkU[2][2] = (Real)1.0;
}
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
// explicit instantiation
//----------------------------------------------------------------------------
namespace Wml
{
template class WML_ITEM Matrix<3,float>;
#ifdef WML_USING_VC6
template WML_ITEM Matrix<3,float> operator* (float,
const Matrix<3,float>&);
template WML_ITEM Vector<3,float> operator*
(const Vector<3,float>&, const Matrix<3,float>&);
#else
template WML_ITEM Matrix<3,float> operator*<3,float> (float,
const Matrix<3,float>&);
template WML_ITEM Vector<3,float> operator*<3,float>
(const Vector<3,float>&, const Matrix<3,float>&);
#endif
template class WML_ITEM Matrix3<float>;
const Matrix3f Matrix3f::ZERO(
0.0f,0.0f,0.0f,
0.0f,0.0f,0.0f,
0.0f,0.0f,0.0f);
const Matrix3f Matrix3f::IDENTITY(
1.0f,0.0f,0.0f,
0.0f,1.0f,0.0f,
0.0f,0.0f,1.0f);
template class WML_ITEM Matrix<3,double>;
#ifdef WML_USING_VC6
template WML_ITEM Matrix<3,double> operator* (double,
const Matrix<3,double>&);
template WML_ITEM Vector<3,double> operator*
(const Vector<3,double>&, const Matrix<3,double>&);
#else
template WML_ITEM Matrix<3,double> operator*<3,double> (double,
const Matrix<3,double>&);
template WML_ITEM Vector<3,double> operator*<3,double>
(const Vector<3,double>&, const Matrix<3,double>&);
#endif
template class WML_ITEM Matrix3<double>;
const Matrix3d Matrix3d::ZERO(
0.0,0.0,0.0,
0.0,0.0,0.0,
0.0,0.0,0.0);
const Matrix3d Matrix3d::IDENTITY(
1.0,0.0,0.0,
0.0,1.0,0.0,
0.0,0.0,1.0);
}
//----------------------------------------------------------------------------
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -