///< Interpolate between 2 quaternions
kmQuaternion* kmQuaternionSlerp(kmQuaternion* pOut,
const kmQuaternion* q1,
const kmQuaternion* q2,
kmScalar t)
{
/*float CosTheta = Q0.DotProd(Q1);
float Theta = acosf(CosTheta);
float SinTheta = sqrtf(1.0f-CosTheta*CosTheta);
float Sin_T_Theta = sinf(T*Theta)/SinTheta;
float Sin_OneMinusT_Theta = sinf((1.0f-T)*Theta)/SinTheta;
Quaternion Result = Q0*Sin_OneMinusT_Theta;
Result += (Q1*Sin_T_Theta);
return Result;*/
if (q1->x == q2->x &&
q1->y == q2->y &&
q1->z == q2->z &&
q1->w == q2->w) {
pOut->x = q1->x;
pOut->y = q1->y;
pOut->z = q1->z;
pOut->w = q1->w;
return pOut;
}
else {
kmScalar ct;
struct kmQuaternion temp;
struct kmQuaternion temp2;
kmScalar theta;
kmScalar st;
kmScalar somt;
kmScalar stt;
ct = kmQuaternionDot(q1, q2);
theta = acosf(ct);
st = sqrtf(1.0 - kmSQR(ct));
stt = sinf(t * theta) / st;
somt = sinf((1.0 - t) * theta) / st;
kmQuaternionScale(&temp, q1, somt);
kmQuaternionScale(&temp2, q2, stt);
kmQuaternionAdd(pOut, &temp, &temp2);
return pOut;
}
}
///< Normalizes a quaternion
kmQuaternion* kmQuaternionNormalize(kmQuaternion* pOut,
const kmQuaternion* pIn)
{
kmScalar length = kmQuaternionLength(pIn);
assert(fabs(length) > kmEpsilon);
kmQuaternionScale(pOut, pIn, 1.0f / length);
return pOut;
}
/**< Interpolate between 2 quaternions*/
kmQuaternion* kmQuaternionSlerp(kmQuaternion* pOut,
const kmQuaternion* q1,
const kmQuaternion* q2,
kmScalar t)
{
kmQuaternion tmp;
kmQuaternion t1, t2;
kmScalar theta_0;
kmScalar theta;
kmScalar dot = kmQuaternionDot(q1, q2);
const double DOT_THRESHOLD = 0.9995;
if (dot > DOT_THRESHOLD) {
kmQuaternion diff;
kmQuaternionSubtract(&diff, q2, q1);
kmQuaternionScale(&diff, &diff, t);
kmQuaternionAdd(pOut, q1, &diff);
kmQuaternionNormalize(pOut, pOut);
return pOut;
}
dot = kmClamp(dot, -1, 1);
theta_0 = acos(dot);
theta = theta_0 * t;
kmQuaternionScale(&tmp, q1, dot);
kmQuaternionSubtract(&tmp, q2, &tmp);
kmQuaternionNormalize(&tmp, &tmp);
kmQuaternionScale(&t1, q1, cos(theta));
kmQuaternionScale(&t2, &tmp, sin(theta));
kmQuaternionAdd(pOut, &t1, &t2);
return pOut;
}
///< Returns the inverse of the passed Quaternion
kmQuaternion* kmQuaternionInverse(kmQuaternion* pOut,
const kmQuaternion* pIn)
{
kmScalar l = kmQuaternionLength(pIn);
kmQuaternion tmp;
if (fabs(l) > kmEpsilon)
{
pOut->x = 0.0;
pOut->y = 0.0;
pOut->z = 0.0;
pOut->w = 0.0;
return pOut;
}
///Get the conjugute and divide by the length
kmQuaternionScale(pOut,
kmQuaternionConjugate(&tmp, pIn), 1.0f / l);
return pOut;
}
