///< 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;
}
}
/**< 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;
}
