appoggio.cpp

Go to the documentation of this file.

 
Quaterniond my_slerp(Quaterniond v0, Quaterniond v1, double t)
{
    // Only unit quaternions are valid rotations.
    // Normalize to avoid undefined behavior.
    v0.normalize();
    v1.normalize();
 
    // Compute the cosine of the angle between the two vectors.
    double dot = v0.dot(v1);
 
    // If the dot product is negative, slerp won't take
    // the shorter path. Note that v1 and -v1 are equivalent when
    // the negation is applied to all four components. Fix by
    // reversing one quaternion.
    if (dot < 0.0f)
    {
        v1 = quaternion_negation(v1);
        dot = -dot;
    }
 
    Quaterniond vdiff = quaternion_minus(v1, v0);
 
    const double DOT_THRESHOLD = 0.9995;
    if (dot > DOT_THRESHOLD)
    {
        // If the inputs are too close for comfort, linearly interpolate
        // and normalize the result.
 
        Quaterniond result = scalar_product(vdiff, t);
        result.normalize();
        return result;
    }
 
    // Since dot is in range [0, DOT_THRESHOLD], acos is safe
    double theta_0 = acos(dot);        // theta_0 = angle between input vectors
    double theta = theta_0 * t;        // theta = angle between v0 and result
    double sin_theta = sin(theta);     // compute this value only once
    double sin_theta_0 = sin(theta_0); // compute this value only once
 
    double s0 =
        cos(theta) - dot * sin_theta / sin_theta_0; // == sin(theta_0 - theta) / sin(theta_0)
    double s1 = sin_theta / sin_theta_0;
 
    return quaternion_plus(scalar_product(v0, s0), scalar_product(v1, s1));
}