quaternion.py 文件源码

def slerp(qa, qb, t):
        # Calculate angle between them.
        #qa.w * qb.w + qa.x * qb.x + qa.y * qb.y + qa.z * qb.z;
        cosHalfTheta = np.dot(qa.q,qb.q)
        #if qa=qb or qa=-qb then theta = 0 and we can return qa
        if (abs(cosHalfTheta) >= 1.0):
            return Quat(np.copy(qa.q));
        #Calculate temporary values.
        halfTheta = acos(cosHalfTheta);
        sinHalfTheta = sqrt(1.0 - cosHalfTheta*cosHalfTheta);

        #if theta = 180 degrees then result is not fully defined
        #we could rotate around any axis normal to qa or qb
        if(abs(sinHalfTheta) < 0.001):
            return Quat(qa.q * 0.5 + qb.q * 0.5);

        ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
        ratioB = sin(t * halfTheta) / sinHalfTheta; 
        #calculate Quaternion for general case.
        return Quat(qa.q * ratioA + qb.q * ratioB);