📄 wm4matrix3.inl
字号:
fCos = Math<Real>::Cos(fXAngle);
fSin = Math<Real>::Sin(fXAngle);
Matrix3 kXMat(
(Real)1.0,(Real)0.0,(Real)0.0,
(Real)0.0,fCos,-fSin,
(Real)0.0,fSin,fCos);
fCos = Math<Real>::Cos(fYAngle);
fSin = Math<Real>::Sin(fYAngle);
Matrix3 kYMat(
fCos,(Real)0.0,fSin,
(Real)0.0,(Real)1.0,(Real)0.0,
-fSin,(Real)0.0,fCos);
*this = kZMat*(kXMat*kYMat);
return *this;
}
//----------------------------------------------------------------------------
template <class Real>
Matrix3<Real>& Matrix3<Real>::FromEulerAnglesZYX (Real fZAngle, Real fYAngle,
Real fXAngle)
{
Real fCos, fSin;
fCos = Math<Real>::Cos(fZAngle);
fSin = Math<Real>::Sin(fZAngle);
Matrix3 kZMat(
fCos,-fSin,(Real)0.0,
fSin,fCos,(Real)0.0,
(Real)0.0,(Real)0.0,(Real)1.0);
fCos = Math<Real>::Cos(fYAngle);
fSin = Math<Real>::Sin(fYAngle);
Matrix3 kYMat(
fCos,(Real)0.0,fSin,
(Real)0.0,(Real)1.0,(Real)0.0,
-fSin,(Real)0.0,fCos);
fCos = Math<Real>::Cos(fXAngle);
fSin = Math<Real>::Sin(fXAngle);
Matrix3 kXMat(
(Real)1.0,(Real)0.0,(Real)0.0,
(Real)0.0,fCos,-fSin,
(Real)0.0,fSin,fCos);
*this = kZMat*(kYMat*kXMat);
return *this;
}
//----------------------------------------------------------------------------
template <class Real>
bool Matrix3<Real>::ToEulerAnglesXYZ (Real& rfXAngle, Real& rfYAngle,
Real& rfZAngle) const
{
// rot = cy*cz -cy*sz sy
// cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
// -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
if (m_afEntry[2] < (Real)1.0)
{
if (m_afEntry[2] > -(Real)1.0)
{
rfXAngle = Math<Real>::ATan2(-m_afEntry[5],m_afEntry[8]);
rfYAngle = (Real)asin((double)m_afEntry[2]);
rfZAngle = Math<Real>::ATan2(-m_afEntry[1],m_afEntry[0]);
return true;
}
else
{
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
rfXAngle = -Math<Real>::ATan2(m_afEntry[3],m_afEntry[4]);
rfYAngle = -Math<Real>::HALF_PI;
rfZAngle = (Real)0.0;
return false;
}
}
else
{
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
rfXAngle = Math<Real>::ATan2(m_afEntry[3],m_afEntry[4]);
rfYAngle = Math<Real>::HALF_PI;
rfZAngle = (Real)0.0;
return false;
}
}
//----------------------------------------------------------------------------
template <class Real>
bool Matrix3<Real>::ToEulerAnglesXZY (Real& rfXAngle, Real& rfZAngle,
Real& rfYAngle) const
{
// rot = cy*cz -sz cz*sy
// sx*sy+cx*cy*sz cx*cz -cy*sx+cx*sy*sz
// -cx*sy+cy*sx*sz cz*sx cx*cy+sx*sy*sz
if (m_afEntry[1] < (Real)1.0)
{
if (m_afEntry[1] > -(Real)1.0)
{
rfXAngle = Math<Real>::ATan2(m_afEntry[7],m_afEntry[4]);
rfZAngle = (Real)asin(-(double)m_afEntry[1]);
rfYAngle = Math<Real>::ATan2(m_afEntry[2],m_afEntry[0]);
return true;
}
else
{
// WARNING. Not unique. XA - YA = atan2(r20,r22)
rfXAngle = Math<Real>::ATan2(m_afEntry[6],m_afEntry[8]);
rfZAngle = Math<Real>::HALF_PI;
rfYAngle = (Real)0.0;
return false;
}
}
else
{
// WARNING. Not unique. XA + YA = atan2(-r20,r22)
rfXAngle = Math<Real>::ATan2(-m_afEntry[6],m_afEntry[8]);
rfZAngle = -Math<Real>::HALF_PI;
rfYAngle = (Real)0.0;
return false;
}
}
//----------------------------------------------------------------------------
template <class Real>
bool Matrix3<Real>::ToEulerAnglesYXZ (Real& rfYAngle, Real& rfXAngle,
Real& rfZAngle) const
{
// rot = cy*cz+sx*sy*sz cz*sx*sy-cy*sz cx*sy
// cx*sz cx*cz -sx
// -cz*sy+cy*sx*sz cy*cz*sx+sy*sz cx*cy
if (m_afEntry[5] < (Real)1.0)
{
if (m_afEntry[5] > -(Real)1.0)
{
rfYAngle = Math<Real>::ATan2(m_afEntry[2],m_afEntry[8]);
rfXAngle = (Real)asin(-(double)m_afEntry[5]);
rfZAngle = Math<Real>::ATan2(m_afEntry[3],m_afEntry[4]);
return true;
}
else
{
// WARNING. Not unique. YA - ZA = atan2(r01,r00)
rfYAngle = Math<Real>::ATan2(m_afEntry[1],m_afEntry[0]);
rfXAngle = Math<Real>::HALF_PI;
rfZAngle = (Real)0.0;
return false;
}
}
else
{
// WARNING. Not unique. YA + ZA = atan2(-r01,r00)
rfYAngle = Math<Real>::ATan2(-m_afEntry[1],m_afEntry[0]);
rfXAngle = -Math<Real>::HALF_PI;
rfZAngle = (Real)0.0;
return false;
}
}
//----------------------------------------------------------------------------
template <class Real>
bool Matrix3<Real>::ToEulerAnglesYZX (Real& rfYAngle, Real& rfZAngle,
Real& rfXAngle) const
{
// rot = cy*cz sx*sy-cx*cy*sz cx*sy+cy*sx*sz
// sz cx*cz -cz*sx
// -cz*sy cy*sx+cx*sy*sz cx*cy-sx*sy*sz
if (m_afEntry[3] < (Real)1.0)
{
if (m_afEntry[3] > -(Real)1.0)
{
rfYAngle = Math<Real>::ATan2(-m_afEntry[6],m_afEntry[0]);
rfZAngle = (Real)asin((double)m_afEntry[3]);
rfXAngle = Math<Real>::ATan2(-m_afEntry[5],m_afEntry[4]);
return true;
}
else
{
// WARNING. Not unique. YA - XA = -atan2(r21,r22);
rfYAngle = -Math<Real>::ATan2(m_afEntry[7],m_afEntry[8]);
rfZAngle = -Math<Real>::HALF_PI;
rfXAngle = (Real)0.0;
return false;
}
}
else
{
// WARNING. Not unique. YA + XA = atan2(r21,r22)
rfYAngle = Math<Real>::ATan2(m_afEntry[7],m_afEntry[8]);
rfZAngle = Math<Real>::HALF_PI;
rfXAngle = (Real)0.0;
return false;
}
}
//----------------------------------------------------------------------------
template <class Real>
bool Matrix3<Real>::ToEulerAnglesZXY (Real& rfZAngle, Real& rfXAngle,
Real& rfYAngle) const
{
// rot = cy*cz-sx*sy*sz -cx*sz cz*sy+cy*sx*sz
// cz*sx*sy+cy*sz cx*cz -cy*cz*sx+sy*sz
// -cx*sy sx cx*cy
if (m_afEntry[7] < (Real)1.0)
{
if (m_afEntry[7] > -(Real)1.0)
{
rfZAngle = Math<Real>::ATan2(-m_afEntry[1],m_afEntry[4]);
rfXAngle = (Real)asin((double)m_afEntry[7]);
rfYAngle = Math<Real>::ATan2(-m_afEntry[6],m_afEntry[8]);
return true;
}
else
{
// WARNING. Not unique. ZA - YA = -atan(r02,r00)
rfZAngle = -Math<Real>::ATan2(m_afEntry[2],m_afEntry[0]);
rfXAngle = -Math<Real>::HALF_PI;
rfYAngle = (Real)0.0;
return false;
}
}
else
{
// WARNING. Not unique. ZA + YA = atan2(r02,r00)
rfZAngle = Math<Real>::ATan2(m_afEntry[2],m_afEntry[0]);
rfXAngle = Math<Real>::HALF_PI;
rfYAngle = (Real)0.0;
return false;
}
}
//----------------------------------------------------------------------------
template <class Real>
bool Matrix3<Real>::ToEulerAnglesZYX (Real& rfZAngle, Real& rfYAngle,
Real& rfXAngle) const
{
// rot = cy*cz cz*sx*sy-cx*sz cx*cz*sy+sx*sz
// cy*sz cx*cz+sx*sy*sz -cz*sx+cx*sy*sz
// -sy cy*sx cx*cy
if (m_afEntry[6] < (Real)1.0)
{
if (m_afEntry[6] > -(Real)1.0)
{
rfZAngle = Math<Real>::ATan2(m_afEntry[3],m_afEntry[0]);
rfYAngle = (Real)asin(-(double)m_afEntry[6]);
rfXAngle = Math<Real>::ATan2(m_afEntry[7],m_afEntry[8]);
return true;
}
else
{
// WARNING. Not unique. ZA - XA = -atan2(r01,r02)
rfZAngle = -Math<Real>::ATan2(m_afEntry[1],m_afEntry[2]);
rfYAngle = Math<Real>::HALF_PI;
rfXAngle = (Real)0.0;
return false;
}
}
else
{
// WARNING. Not unique. ZA + XA = atan2(-r01,-r02)
rfZAngle = Math<Real>::ATan2(-m_afEntry[1],-m_afEntry[2]);
rfYAngle = -Math<Real>::HALF_PI;
rfXAngle = (Real)0.0;
return false;
}
}
//----------------------------------------------------------------------------
template <class Real>
Matrix3<Real>& Matrix3<Real>::Slerp (Real fT, const Matrix3& rkR0,
const Matrix3& rkR1)
{
Vector3<Real> kAxis;
Real fAngle;
Matrix3 kProd = rkR0.TransposeTimes(rkR1);
kProd.ToAxisAngle(kAxis,fAngle);
FromAxisAngle(kAxis,fT*fAngle);
return *this;
}
//----------------------------------------------------------------------------
template <class Real>
bool Matrix3<Real>::Tridiagonalize (Real afDiag[3], Real afSubd[2])
{
// Householder reduction T = Q^t M Q
// Input:
// mat, symmetric 3x3 matrix M
// Output:
// mat, orthogonal matrix Q (a reflection)
// diag, diagonal entries of T
// subd, subdiagonal entries of T (T is symmetric)
Real fM00 = m_afEntry[0];
Real fM01 = m_afEntry[1];
Real fM02 = m_afEntry[2];
Real fM11 = m_afEntry[4];
Real fM12 = m_afEntry[5];
Real fM22 = m_afEntry[8];
afDiag[0] = fM00;
if (Math<Real>::FAbs(fM02) >= Math<Real>::ZERO_TOLERANCE)
{
afSubd[0] = Math<Real>::Sqrt(fM01*fM01+fM02*fM02);
Real fInvLength = ((Real)1.0)/afSubd[0];
fM01 *= fInvLength;
fM02 *= fInvLength;
Real fTmp = ((Real)2.0)*fM01*fM12+fM02*(fM22-fM11);
afDiag[1] = fM11+fM02*fTmp;
afDiag[2] = fM22-fM02*fTmp;
afSubd[1] = fM12-fM01*fTmp;
m_afEntry[0] = (Real)1.0;
m_afEntry[1] = (Real)0.0;
m_afEntry[2] = (Real)0.0;
m_afEntry[3] = (Real)0.0;
m_afEntry[4] = fM01;
m_afEntry[5] = fM02;
m_afEntry[6] = (Real)0.0;
m_afEntry[7] = fM02;
m_afEntry[8] = -fM01;
return true;
}
else
{
afDiag[1] = fM11;
afDiag[2] = fM22;
afSubd[0] = fM01;
afSubd[1] = fM12;
m_afEntry[0] = (Real)1.0;
m_afEntry[1] = (Real)0.0;
m_afEntry[2] = (Real)0.0;
m_afEntry[3] = (Real)0.0;
m_afEntry[4] = (Real)1.0;
m_afEntry[5] = (Real)0.0;
m_afEntry[6] = (Real)0.0;
m_afEntry[7] = (Real)0.0;
m_afEntry[8] = (Real)1.0;
return false;
}
}
//----------------------------------------------------------------------------
template <class Real>
bool Matrix3<Real>::QLAlgorithm (Real afDiag[3], Real afSubd[2])
{
// This is an implementation of the symmetric QR algorithm from the book
// "Matrix Computations" by Gene H. Golub and Charles F. Van Loan, second
// edition. The algorithm is 8.2.3. The implementation has a slight
// variation to actually make it a QL algorithm, and it traps the case
// when either of the subdiagonal terms s0 or s1 is zero and reduces the
// 2-by-2 subblock directly.
const int iMax = 32;
for (int i = 0; i < iMax; i++)
{
Real fSum, fDiff, fDiscr, fEValue0, fEValue1, fCos, fSin, fTmp;
int iRow;
fSum = Math<Real>::FAbs(afDiag[0]) + Math<Real>::FAbs(afDiag[1]);
if (Math<Real>::FAbs(afSubd[0]) + fSum == fSum)
{
// The matrix is effectively
// +- -+
// M = | d0 0 0 |
// | 0 d1 s1 |
// | 0 s1 d2 |
// +- -+
// Compute the eigenvalues as roots of a quadratic equation.
fSum = afDiag[1] + afDiag[2];
fDiff = afDiag[1] - afDiag[2];
fDiscr = Math<Real>::Sqrt(fDiff*fDiff +
((Real)4.0)*afSubd[1]*afSubd[1]);
fEValue0 = ((Real)0.5)*(fSum - fDiscr);
fEValue1 = ((Real)0.5)*(fSum + fDiscr);
// Compute the Givens rotation.
if (fDiff >= (Real)0.0)
{
fCos = afSubd[1];
fSin = afDiag[1] - fEValue0;
}
else
{
fCos = afDiag[2] - fEValue0;
fSin = afSubd[1];
}
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++)
{
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -