faster_slerp.rs

faster_slerp.rs

use directx_math::*;

// http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/

// simd version:

fn slerp(mut Q0: XMVECTOR, Q1: XMVECTOR, t: f32) -> XMVECTOR {
    const DOT_THRESHOLD: XMVECTORF32 = XMVECTORF32 { f: [0.9995, 0.9995, 0.9995, 0.9995] };

    let mut dot = XMQuaternionDot(Q0, Q1);

    if XMVector4Greater(dot, *DOT_THRESHOLD) {
        XMQuaternionSlerp(Q0, Q1, t)
    } else {
        // https://stackoverflow.com/questions/2886606/flipping-issue-when-interpolating-rotations-using-quaternions/2887128#2887128
        //
        // > Remember, each rotation can actually be represented by two quaternions, q and -q. 
        // > But the Slerp path from q to w will be different from the path from (-q) to w: one 
        // > will go the long away around, the other the short away around. It sounds like you're
        // > getting the long way when you want the short way.
        // > Try taking the dot product of your two quaternions (i.e., the 4-D dot product), and
        // > if the dot product is negative, replace your quaterions q1 and q2 with -q1 and q2
        // > before performing Slerp.
        if XMVector4Less(dot, *g_XMZero) {
            Q0 = XMVectorNegate(Q0);
            dot = XMQuaternionDot(Q0, Q1);
        }

        let theta = XMVectorACosEst(dot);
        let x = 1.0 - t;
        let y = t;
        let z = 1.0;
        let tmp = XMVectorMultiply(theta, XMVectorSet(x, y, z, 1.0));
        let tmp = XMVectorSinEst(tmp);

        let scale1 = <(SwizzleX, SwizzleX, SwizzleX, SwizzleX)>::XMVectorSwizzle(tmp);
        let scale2 = <(SwizzleY, SwizzleY, SwizzleY, SwizzleY)>::XMVectorSwizzle(tmp);
        let theta_sin = <(SwizzleZ, SwizzleZ, SwizzleZ, SwizzleZ)>::XMVectorSwizzle(tmp);
        let theta_sin_recip = XMVectorReciprocal(theta_sin);

        let tmp2 = XMVectorMultiply(Q0, scale1);
        let tmp3 = XMVectorMultiply(Q1, scale2);
        let tmp4 = XMVectorAdd(tmp2, tmp3);

        XMVectorMultiply(tmp4, theta_sin_recip)
    }
}

// from cgmath:

fn slerp(self, other: Quaternion<S>, amount: S) -> Quaternion<S> {
    let dot = self.dot(other);
    let dot_threshold = cast(0.9995f64).unwrap();

    // if quaternions are close together use `nlerp`
    if dot > dot_threshold {
        self.nlerp(other, amount)
    } else {
        // stay within the domain of acos()
        // TODO REMOVE WHEN https://github.com/mozilla/rust/issues/12068 IS RESOLVED
        let robust_dot = if dot > S::one() {
            S::one()
        } else if dot < -S::one() {
            -S::one()
        } else {
            dot
        };

        let theta = Rad::acos(robust_dot.clone());

        let scale1 = Rad::sin(theta * (S::one() - amount));
        let scale2 = Rad::sin(theta * amount);

        (self * scale1 + other * scale2) * Rad::sin(theta).recip()
    }
}