// -----------------------------------------------------------------------------------------
CPepeEngineQuaternion CPepeEngineQuaternion::log() const
{
// If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x,y,z) is unit length, then
// log(q) = A*(x*i+y*j+z*k). If sin(A) is near zero, use log(q) =
// sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.
CPepeEngineQuaternion kResult;
kResult.w = 0.0;
if (CPepeEngineMath::Abs(w) < 1.0) {
Radian fAngle(CPepeEngineMath::ACos(w));
float fSin = CPepeEngineMath::Sin(fAngle);
if (CPepeEngineMath::Abs(fSin) >= ms_fEpsilon) {
float fCoeff = fAngle.valueRadians() / fSin;
kResult.x = fCoeff * x;
kResult.y = fCoeff * y;
kResult.z = fCoeff * z;
return kResult;
}
}
kResult.x = x;
kResult.y = y;
kResult.z = z;
return kResult;
}
// -----------------------------------------------------------------------------------------
CPepeEngineQuaternion CPepeEngineQuaternion::exp() const
{
// If q = A*(x*i+y*j+z*k) where (x,y,z) is unit length, then
// exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k). If sin(A) is near zero,
// use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.
Radian fAngle(CPepeEngineMath::Sqrt(x * x + y * y + z * z));
float fSin = CPepeEngineMath::Sin(fAngle);
CPepeEngineQuaternion kResult;
kResult.w = CPepeEngineMath::Cos(fAngle);
if (CPepeEngineMath::Abs(fSin) >= ms_fEpsilon) {
float fCoeff = fSin / (fAngle.valueRadians());
kResult.x = fCoeff * x;
kResult.y = fCoeff * y;
kResult.z = fCoeff * z;
} else {
kResult.x = x;
kResult.y = y;
kResult.z = z;
}
return kResult;
}
//-----------------------------------------------------------------------
Quaternion Quaternion::Log() const
{
// 如果 q = cos(A)+sin(A)*(x*i+y*j+z*k) 而 (x,y,z) 是单位长度, 那么
// log(q) = A*(x*i+y*j+z*k). 如果 sin(A) 接近0, 用 log(q) =
// sin(A)*(x*i+y*j+z*k) 自 sin(A)/A 极限为 1.
Quaternion kResult;
kResult.w = 0.0;
if (Math::Abs(w) < 1.0)
{
Radian fAngle(Math::ACos(w));
Real fSin = Math::Sin(fAngle);
if (Math::Abs(fSin) >= msEpsilon)
{
Real fCoeff = fAngle.valueRadians() / fSin;
kResult.x = fCoeff*x;
kResult.y = fCoeff*y;
kResult.z = fCoeff*z;
return kResult;
}
}
kResult.x = x;
kResult.y = y;
kResult.z = z;
return kResult;
}
//-----------------------------------------------------------------------
Quaternion Quaternion::Exp() const
{
// 如果 q = A*(x*i+y*j+z*k) 而 (x,y,z) 是单位长度, 那么
// exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k). 如 sin(A) 接近0
// 用 exp(q) = cos(A)+A*(x*i+y*j+z*k) 自 A/sin(A) 极限为 1.
Radian fAngle(Math::Sqrt(x*x + y*y + z*z));
Real fSin = Math::Sin(fAngle);
Quaternion kResult;
kResult.w = Math::Cos(fAngle);
if (Math::Abs(fSin) >= msEpsilon)
{
Real fCoeff = fSin / (fAngle.valueRadians());
kResult.x = fCoeff*x;
kResult.y = fCoeff*y;
kResult.z = fCoeff*z;
}
else
{
kResult.x = x;
kResult.y = y;
kResult.z = z;
}
return kResult;
}
//-----------------------------------------------------------------------
Quaternion Quaternion::SlerpExtraSpins(Real fT,
const Quaternion& rkP, const Quaternion& rkQ, int iExtraSpins)
{
Real fCos = rkP.Dot(rkQ);
Radian fAngle(Math::ACos(fCos));
if (Math::Abs(fAngle.valueRadians()) < msEpsilon)
return rkP;
Real fSin = Math::Sin(fAngle);
Radian fPhase(Math::_PI*iExtraSpins*fT);
Real fInvSin = 1.0f / fSin;
Real fCoeff0 = Math::Sin((1.0f - fT)*fAngle.valueRadians() - fPhase.valueRadians())*fInvSin;
Real fCoeff1 = Math::Sin(fT*fAngle.valueRadians() + fPhase.valueRadians())*fInvSin;
return fCoeff0*rkP + fCoeff1*rkQ;
}
// -----------------------------------------------------------------------------------------
CPepeEngineQuaternion CPepeEngineQuaternion::slerpExtraSpins (
float fT, const CPepeEngineQuaternion& rkP, const CPepeEngineQuaternion& rkQ, int iExtraSpins)
{
float fCos = rkP.dot(rkQ);
Radian fAngle(CPepeEngineMath::ACos(fCos));
if (CPepeEngineMath::Abs(fAngle.valueRadians()) < ms_fEpsilon) {
return rkP;
}
float fSin = CPepeEngineMath::Sin(fAngle);
Radian fPhase(CPepeEngineMath::PI * iExtraSpins * fT);
float fInvSin = 1.0f / fSin;
float fCoeff0 = CPepeEngineMath::Sin((1.0f - fT) * fAngle - fPhase) * fInvSin;
float fCoeff1 = CPepeEngineMath::Sin(fT * fAngle + fPhase) * fInvSin;
return fCoeff0 * rkP + fCoeff1 * rkQ;
}
Quaternion Quaternion::Exp() const
{
Radian fAngle(Math::Sqrt(x * x + y * y + z *z));
float fSin = Math::Sin(fAngle);
Quaternion kResult;
if (Math::Abs(fSin) >= msEpsilon)
{
float fCoeff = fSin / (fAngle.valueRadians());
kResult.x = fCoeff*x;
kResult.y = fCoeff*y;
kResult.z = fCoeff*z;
}
else
{
kResult.x = x;
kResult.y = y;
kResult.z = z;
}
return kResult;
}
Quaternion Quaternion::Log() const
{
Quaternion kResult;
kResult.w = 0.0;
if (Math::Abs(w) < 1.0)
{
Radian fAngle(Math::ACos(w));
float fSin = Math::Sin(fAngle);
if (Math::Abs(fSin) >= msEpsilon)
{
float fCoeff = fAngle.valueRadians() / fSin;
kResult.x = fCoeff * x;
kResult.y = fCoeff * y;
kResult.z = fCoeff * z;
return kResult;
}
}
kResult.x = x;
kResult.y = y;
kResult.z = z;
return kResult;
}
