📄 wmlmatrix3.inl
字号:
// compute q0
Real fInvLength = Math<Real>::InvSqrt(m_afEntry[0]*m_afEntry[0] +
m_afEntry[3]*m_afEntry[3] + m_afEntry[6]*m_afEntry[6]);
m_afEntry[0] *= fInvLength;
m_afEntry[3] *= fInvLength;
m_afEntry[6] *= fInvLength;
// compute q1
Real fDot0 = m_afEntry[0]*m_afEntry[1] + m_afEntry[3]*m_afEntry[4] +
m_afEntry[6]*m_afEntry[7];
m_afEntry[1] -= fDot0*m_afEntry[0];
m_afEntry[4] -= fDot0*m_afEntry[3];
m_afEntry[7] -= fDot0*m_afEntry[6];
fInvLength = Math<Real>::InvSqrt(m_afEntry[1]*m_afEntry[1] +
m_afEntry[4]*m_afEntry[4] + m_afEntry[7]*m_afEntry[7]);
m_afEntry[1] *= fInvLength;
m_afEntry[4] *= fInvLength;
m_afEntry[7] *= fInvLength;
// compute q2
Real fDot1 = m_afEntry[1]*m_afEntry[2] + m_afEntry[4]*m_afEntry[5] +
m_afEntry[7]*m_afEntry[8];
fDot0 = m_afEntry[0]*m_afEntry[2] + m_afEntry[3]*m_afEntry[5] +
m_afEntry[6]*m_afEntry[8];
m_afEntry[2] -= fDot0*m_afEntry[0] + fDot1*m_afEntry[1];
m_afEntry[5] -= fDot0*m_afEntry[3] + fDot1*m_afEntry[4];
m_afEntry[8] -= fDot0*m_afEntry[6] + fDot1*m_afEntry[7];
fInvLength = Math<Real>::InvSqrt(m_afEntry[2]*m_afEntry[2] +
m_afEntry[5]*m_afEntry[5] + m_afEntry[8]*m_afEntry[8]);
m_afEntry[2] *= fInvLength;
m_afEntry[5] *= fInvLength;
m_afEntry[8] *= fInvLength;
}
//----------------------------------------------------------------------------
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>
void Matrix3<Real>::FromEulerAnglesXYZ (Real fYAngle, Real fPAngle,
Real fRAngle)
{
Real fCos, fSin;
fCos = Math<Real>::Cos(fYAngle);
fSin = Math<Real>::Sin(fYAngle);
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(fPAngle);
fSin = Math<Real>::Sin(fPAngle);
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(fRAngle);
fSin = Math<Real>::Sin(fRAngle);
Matrix3 kZMat(
fCos,-fSin,(Real)0.0,
fSin,fCos,(Real)0.0,
(Real)0.0,(Real)0.0,(Real)1.0);
*this = kXMat*(kYMat*kZMat);
}
//----------------------------------------------------------------------------
template <class Real>
void Matrix3<Real>::FromEulerAnglesXZY (Real fYAngle, Real fPAngle,
Real fRAngle)
{
Real fCos, fSin;
fCos = Math<Real>::Cos(fYAngle);
fSin = Math<Real>::Sin(fYAngle);
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(fPAngle);
fSin = Math<Real>::Sin(fPAngle);
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(fRAngle);
fSin = Math<Real>::Sin(fRAngle);
Matrix3 kYMat(
fCos,(Real)0.0,fSin,
(Real)0.0,(Real)1.0,(Real)0.0,
-fSin,(Real)0.0,fCos);
*this = kXMat*(kZMat*kYMat);
}
//----------------------------------------------------------------------------
template <class Real>
void Matrix3<Real>::FromEulerAnglesYXZ (Real fYAngle, Real fPAngle,
Real fRAngle)
{
Real fCos, fSin;
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(fPAngle);
fSin = Math<Real>::Sin(fPAngle);
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(fRAngle);
fSin = Math<Real>::Sin(fRAngle);
Matrix3 kZMat(
fCos,-fSin,(Real)0.0,
fSin,fCos,(Real)0.0,
(Real)0.0,(Real)0.0,(Real)1.0);
*this = kYMat*(kXMat*kZMat);
}
//----------------------------------------------------------------------------
template <class Real>
void Matrix3<Real>::FromEulerAnglesYZX (Real fYAngle, Real fPAngle,
Real fRAngle)
{
Real fCos, fSin;
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(fPAngle);
fSin = Math<Real>::Sin(fPAngle);
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(fRAngle);
fSin = Math<Real>::Sin(fRAngle);
Matrix3 kXMat(
(Real)1.0,(Real)0.0,(Real)0.0,
(Real)0.0,fCos,-fSin,
(Real)0.0,fSin,fCos);
*this = kYMat*(kZMat*kXMat);
}
//----------------------------------------------------------------------------
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -