四元数的球面线性插值(slerp) - 知乎 (zhihu.com)
void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3)
{
double norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 /= norm;
q1 /= norm;
q2 /= norm;
q3 /= norm;
}
void scaleQuaternion(double gain,
double& dq0, double& dq1, double& dq2, double& dq3)
{
if (dq0 < 0.0)//0.9
{
// Slerp (Spherical linear interpolation):
double angle = acos(dq0);
double A = sin(angle*(1.0 - gain))/sin(angle);
double B = sin(angle * gain)/sin(angle);
dq0 = A + B * dq0;
dq1 = B * dq1;
dq2 = B * dq2;
dq3 = B * dq3;
}
else
{
// Lerp (Linear interpolation):
dq0 = (1.0 - gain) + gain * dq0;
dq1 = gain * dq1;
dq2 = gain * dq2;
dq3 = gain * dq3;
}
normalizeQuaternion(dq0, dq1, dq2, dq3);
}