quaternion quaternion::SlerpExtraSpins(real fT, const quaternion& rkP, const quaternion& rkQ, int iExtraSpins)
{
real fCos = rkP.Dot(rkQ);
real fAngle (math::acos(fCos) );
if (math::abs(fAngle) < msEpsilon )
return rkP;
real fSin = math::sin(fAngle);
real fPhase ( math::pi*iExtraSpins*fT );
real fInvSin = 1.0f/fSin;
real fCoeff0 = math::sin((1.0f-fT)*fAngle - fPhase)*fInvSin;
real fCoeff1 = math::sin(fT*fAngle + fPhase)*fInvSin;
return fCoeff0*rkP + fCoeff1*rkQ;
}
quaternion quaternion::nlerp(real fT, const quaternion &rkP, const quaternion &rkQ, bool shortestPath)
{
quaternion result;
real fCos = rkP.Dot(rkQ);
if (fCos < 0.0f && shortestPath)
{
result = rkP + fT * ((-rkQ) - rkP);
}
else
{
result = rkP + fT * (rkQ - rkP);
}
result.normalise();
return result;
}
quaternion quaternion::Slerp(real fT, const quaternion &rkP, const quaternion &rkQ, bool shortestPath)
{
real fCos = rkP.Dot(rkQ);
quaternion rkT;
// Do we need to invert rotation?
if (fCos < 0.0f && shortestPath)
{
fCos = -fCos;
rkT = -rkQ;
}
else
{
rkT = rkQ;
}
if (math::abs(fCos) < 1 - msEpsilon)
{
// Standard case (slerp)
real fSin = math::sqrt(1 - fCos*fCos);
real fAngle = math::atan2(fSin, fCos);
real fInvSin = 1.0f / fSin;
real fCoeff0 = math::sin((1.0f - fT) * fAngle) * fInvSin;
real fCoeff1 = math::sin(fT * fAngle) * fInvSin;
return fCoeff0 * rkP + fCoeff1 * rkT;
}
else
{
// There are two situations:
// 1. "rkP" and "rkQ" are very close (fCos ~= +1), so we can do a linear
// interpolation safely.
// 2. "rkP" and "rkQ" are almost inverse of each other (fCos ~= -1), there
// are an infinite number of possibilities interpolation. but we haven't
// have method to fix this case, so just use linear interpolation here.
quaternion t = (1.0f - fT) * rkP + fT * rkT;
// taking the complement requires renormalisation
t.normalise();
return t;
}
}
