ARCore中四元数的插值算法实现

ARCore中四元数差值算法：

其中t的取值范围为[0, 1]，当 t = 0 时，结果为a；当t = 1 时，结果为b。

　　public static Quaternion makeInterpolated(Quaternion a, Quaternion b, float t) {
        Quaternion out = new Quaternion();
        float cosHalfTheta = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
        if(cosHalfTheta < 0.0F) {
            b = new Quaternion(b);
            cosHalfTheta = -cosHalfTheta;
            b.x = -b.x;
            b.y = -b.y;
            b.z = -b.z;
            b.w = -b.w;
        }

        float halfTheta = (float)Math.acos((double)cosHalfTheta);
        float sinHalfTheta = (float)Math.sqrt((double)(1.0F - cosHalfTheta * cosHalfTheta));
        float ratioA;
        float ratioB;
        if((double)Math.abs(sinHalfTheta) > 0.001D) {
            float oneOverSinHalfTheta = 1.0F / sinHalfTheta;
            ratioA = (float)Math.sin((double)((1.0F - t) * halfTheta)) * oneOverSinHalfTheta;
            ratioB = (float)Math.sin((double)(t * halfTheta)) * oneOverSinHalfTheta;
        } else {
            ratioA = 1.0F - t;
            ratioB = t;
        }

        out.x = ratioA * a.x + ratioB * b.x;
        out.y = ratioA * a.y + ratioB * b.y;
        out.z = ratioA * a.z + ratioB * b.z;
        out.w = ratioA * a.w + ratioB * b.w;
        out.normalizeInPlace();
        return out;
    }