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