Repository: bitshifter/glam-rs
Pull request #57 (2020-04-17 00:23:24)

Add Quat::slerp
This PR adds `Quat::slerp`. This implementation is derived from the cgmath implementation which is based on http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/

Unlike the cgmath implementation , intrinsics are used here when available.

Opening as draft to gain feedback.

1. `_MM_SHUFFLE` is considerably more readable than the duplication of the binary constants. Although it's implemented in the rust std library, it's currently nightly only. I've copied that implementation here.
2. I've added a few macros that mirror the DirectXMath macros, but I'm not sure if these are something you'll want in the code.
3. I've used the DirectXMath constant names which aren't snake case. I think it makes it easier when comparing the code, but I can understand if you wouldn't want that.

aloucks: @bitshifter I've addressed all the feedback and added some tests for slerp. 
aloucks: `_mm_permute_ps` and `_mm_shuffle_ps` have `#[rustc_args_required_const(2)]` attribute set to enforce that the mask is constant. Using a macro is the only option.

```rust
pub(crate) unsafe fn m128_permute(a: __m128, mask: i32) -> __m128 {
    _mm_permute_ps(a, mask)
}
```

```
error: argument 2 is required to be a constant
   --> src\f32\funcs.rs:124:9
    |
124 |         _mm_permute_ps(a, mask)
    | 
```
aloucks: @bitshifter I've added doc comments on `m128_sin` to explain that `sin` is calculated in each lane and that the error increases as the input drifts further from `[-PI, PI]`. 

The function is suitable for use in `slerp` as we know the input will always be within these bounds. I've also enabled the angle modulus so that it's now  implemented exactly as `XMVectorSin`. I compared this with the `C++` version of `XMVectorSin` and observed the same behavior.

I can remove the commented out test that attempts to test a larger range (and fails), if you'd like. I also checked using `f32::sin()` for each lane  and `slerp` was about twice as slow.

I have not updated vec3/vec4 to take advantage of the fused multiply add/sub. I started to do so, but I ran into an issue with importing the the macros and I didn't want to start re-organizing things in this PR.

Patch:
diff --git a/benches/quat.rs b/benches/quat.rs
index 4d06fb9..3c6d408 100644
--- a/benches/quat.rs
+++ b/benches/quat.rs
@@ -22,6 +22,13 @@ bench_binop!(
     from => random_quat
 );
 
+bench_binop!(
+    quat_dot,
+    "quat dot",
+    op => dot,
+    from => random_quat
+);
+
 bench_trinop!(
     quat_lerp,
     "quat lerp",
@@ -31,12 +38,23 @@ bench_trinop!(
     from3 => random_f32
 );
 
+bench_trinop!(
+    quat_slerp,
+    "quat slerp",
+    op => slerp,
+    from1 => random_quat,
+    from2 => random_quat,
+    from3 => random_f32
+);
+
 bench_from_ypr!(quat_from_ypr, "quat from ypr", ty => Quat);
 
 criterion_group!(
     benches,
     quat_conjugate,
+    quat_dot,
     quat_lerp,
+    quat_slerp,
     quat_mul_quat,
     quat_from_ypr
 );
diff --git a/src/f32/funcs.rs b/src/f32/funcs.rs
index bed72b7..24cbdd0 100644
--- a/src/f32/funcs.rs
+++ b/src/f32/funcs.rs
@@ -59,6 +59,13 @@ pub(crate) mod sse2 {
                 $field: [$x, $x, $x, $x],
             };
         };
+
+        ($name:ident, $field:ident, $x:expr, $y:expr, $z:expr, $w:expr) => {
+            #[cfg(any(target_arch = "x86", target_arch = "x86_64"))]
+            const $name: UnionCast = UnionCast {
+                $field: [$x, $y, $z, $w],
+            };
+        };
     }
 
     _ps_const_ty!(PS_INV_SIGN_MASK, u32x4, !0x8000_0000);
@@ -84,6 +91,57 @@ pub(crate) mod sse2 {
     // _ps_const_ty!(PS_COSCOF_P2, f32x4, 4.166_664_6e-2);
     // _ps_const_ty!(PS_CEPHES_FOPI, f32x4, 1.273_239_5); // 4 / M_PI
 
+    _ps_const_ty!(PS_NEGATIVE_ZERO, u32x4, 0x80000000);
+    _ps_const_ty!(PS_PI, f32x4, std::f32::consts::PI);
+    _ps_const_ty!(PS_HALF_PI, f32x4, std::f32::consts::FRAC_PI_2);
+    _ps_const_ty!(
+        PS_SIN_COEFFICIENTS0,
+        f32x4,
+        -0.16666667,
+        0.0083333310,
+        -0.00019840874,
+        2.7525562e-06
+    );
+    _ps_const_ty!(
+        PS_SIN_COEFFICIENTS1,
+        f32x4,
+        -2.3889859e-08,
+        -0.16665852,    /*Est1*/
+        0.0083139502,   /*Est2*/
+        -0.00018524670  /*Est3*/
+    );
+    _ps_const_ty!(PS_ONE, f32x4, 1.0);
+    _ps_const_ty!(PS_TWO_PI, f32x4, std::f32::consts::PI * 2.0);
+    _ps_const_ty!(PS_RECIPROCAL_TWO_PI, f32x4, 0.159154943);
+
+    #[cfg(target_feature = "fma")]
+    macro_rules! m128_mul_add {
+        ($a:expr, $b:expr, $c:expr) => {
+            _mm_fmadd_ps($a, $b, $c)
+        };
+    }
+
+    #[cfg(not(target_feature = "fma"))]
+    macro_rules! m128_mul_add {
+        ($a:expr, $b:expr, $c:expr) => {
+            _mm_add_ps(_mm_mul_ps($a, $b), $c)
+        };
+    }
+
+    #[cfg(target_feature = "fma")]
+    macro_rules! m128_neg_mul_sub {
+        ($a:expr, $b:expr, $c:expr) => {
+            _mm_fnmadd_ps($a, $b, $c)
+        };
+    }
+
+    #[cfg(not(target_feature = "fma"))]
+    macro_rules! m128_neg_mul_sub {
+        ($a:expr, $b:expr, $c:expr) => {
+            _mm_sub_ps($c, _mm_mul_ps($a, $b))
+        };
+    }
+
     #[inline]
     pub(crate) unsafe fn m128_round(v: __m128) -> __m128 {
         // From DirectXMath XMVectorRound.
@@ -138,6 +196,64 @@ pub(crate) mod sse2 {
         _mm_or_ps(result, _mm_castsi128_ps(test))
     }
 
+    /// Returns a vector whose components are the corresponding components of Angles modulo 2PI.
+    #[inline]
+    pub(crate) unsafe fn m128_mod_angles(angles: __m128) -> __m128 {
+        // From DirectXMath: XMVectorModAngles
+        let v = _mm_mul_ps(angles, PS_RECIPROCAL_TWO_PI.m128);
+        let v = m128_round(v);
+        m128_neg_mul_sub!(PS_TWO_PI.m128, v, angles)
+    }
+
+    /// Computes the sine of the angle in each lane of `v`. Values outside
+    /// the bounds of PI may produce an increasing error as the input angle
+    /// drifts from `[-PI, PI]`.
+    #[inline]
+    pub(crate) unsafe fn m128_sin(v: __m128) -> __m128 {
+        // From DirectXMath: XMVectorSin
+        //
+        // 11-degree minimax approximation
+        //
+        // Force the value within the bounds of pi
+        let mut x = m128_mod_angles(v);
+
+        // Map in [-pi/2,pi/2] with sin(y) = sin(x).
+        let sign = _mm_and_ps(x, PS_NEGATIVE_ZERO.m128);
+        // pi when x >= 0, -pi when x < 0
+        let c = _mm_or_ps(PS_PI.m128, sign);
+        // |x|
+        let absx = _mm_andnot_ps(sign, x);
+        let rflx = _mm_sub_ps(c, x);
+        let comp = _mm_cmple_ps(absx, PS_HALF_PI.m128);
+        let select0 = _mm_and_ps(comp, x);
+        let select1 = _mm_andnot_ps(comp, rflx);
+        x = _mm_or_ps(select0, select1);
+
+        let x2 = _mm_mul_ps(x, x);
+
+        // Compute polynomial approximation
+        const SC1: __m128 = unsafe { PS_SIN_COEFFICIENTS1.m128 };
+        let v_constants_b = _mm_shuffle_ps(SC1, SC1, 0b00_00_00_00);
+
+        const SC0: __m128 = unsafe { PS_SIN_COEFFICIENTS0.m128 };
+        let mut v_constants = _mm_shuffle_ps(SC0, SC0, 0b11_11_11_11);
+        let mut result = m128_mul_add!(v_constants_b, x2, v_constants);
+
+        v_constants = _mm_shuffle_ps(SC0, SC0, 0b10_10_10_10);
+        result = m128_mul_add!(result, x2, v_constants);
+
+        v_constants = _mm_shuffle_ps(SC0, SC0, 0b01_01_01_01);
+        result = m128_mul_add!(result, x2, v_constants);
+
+        v_constants = _mm_shuffle_ps(SC0, SC0, 0b00_00_00_00);
+        result = m128_mul_add!(result, x2, v_constants);
+
+        result = m128_mul_add!(result, x2, PS_ONE.m128);
+        result = _mm_mul_ps(result, x);
+
+        result
+    }
+
     // Based on http://gruntthepeon.free.fr/ssemath/sse_mathfun.h
     // #[cfg(target_feature = "sse2")]
     // unsafe fn sin_cos_sse2(x: __m128) -> (__m128, __m128) {
@@ -348,3 +464,87 @@ fn test_scalar_sin_cos() {
     assert!(s.is_nan());
     assert!(c.is_nan());
 }
+
+#[test]
+#[cfg(vec4sse2)]
+fn test_sse2_m128_sin() {
+    use crate::Vec4;
+    use std::f32::consts::PI;
+
+    fn test_sse2_m128_sin_angle(a: f32) {
+        let v = Vec4::splat(a);
+        let v = unsafe { Vec4(sse2::m128_sin(v.0)) };
+        let a_sin = a.sin();
+        dbg!((a, a_sin, v));
+        assert_approx_eq!(v.x(), a_sin, 1e-6);
+        assert_approx_eq!(v.z(), a_sin, 1e-6);
+        assert_approx_eq!(v.y(), a_sin, 1e-6);
+        assert_approx_eq!(v.w(), a_sin, 1e-6);
+    }
+
+    let mut a = -PI;
+    let end = PI;
+    let step = PI / 8192.0;
+
+    while a <= end {
+        test_sse2_m128_sin_angle(a);
+        a += step;
+    }
+}
+
+// sse2::m128_sin is derived from the XMVectorSin in DirectXMath. It's been
+// observed both here and in the C++ version that the error rate increases
+// as the input angle drifts further from the bounds of PI.
+//
+// #[test]
+// #[cfg(vec4sse2)]
+// fn test_sse2_m128_sin2() {
+//     use crate::Vec4;
+
+//     fn test_sse2_m128_sin_angle(a: f32) -> f32 {
+//         let v = Vec4::splat(a);
+//         let v = unsafe { Vec4(sse2::m128_sin(v.0)) };
+//         let a_sin = a.sin();
+//         let v_sin = v.x();
+//         // println!("{:?}", (a, a_sin, v_sin));
+//         assert_approx_eq!(a_sin, v.x(), 1e-4);
+//         assert_approx_eq!(a_sin, v.z(), 1e-4);
+//         assert_approx_eq!(a_sin, v.y(), 1e-4);
+//         assert_approx_eq!(a_sin, v.w(), 1e-4);
+//         v.x()
+//     }
+
+//     // test 1024 floats between -PI and PI inclusive
+//     const MAX_TESTS: u32 = 1024 / 2;
+//     const SIGN: u32 = 0x80_00_00_00;
+//     let ptve_pi = std::f32::consts::PI.to_bits();
+//     let ngve_pi = SIGN | ptve_pi;
+//     let step_pi = (ptve_pi / MAX_TESTS) as usize;
+//     for f in (SIGN..=ngve_pi).step_by(step_pi).map(|i| f32::from_bits(i)) {
+//         test_sse2_m128_sin_angle(f);
+//     }
+//     for f in (0..=ptve_pi).step_by(step_pi).map(|i| f32::from_bits(i)) {
+//         test_sse2_m128_sin_angle(f);
+//     }
+
+//     // test 1024 floats between -INF and +INF exclusive
+//     let ptve_inf = std::f32::INFINITY.to_bits();
+//     let ngve_inf = std::f32::NEG_INFINITY.to_bits();
+//     let step_inf = (ptve_inf / MAX_TESTS) as usize;
+//     for f in (SIGN..ngve_inf)
+//         .step_by(step_inf)
+//         .map(|i| f32::from_bits(i))
+//     {
+//         test_sse2_m128_sin_angle(f);
+//     }
+//     for f in (0..ptve_inf).step_by(step_inf).map(|i| f32::from_bits(i)) {
+//         test_sse2_m128_sin_angle(f);
+//     }
+
+//     // +inf and -inf should return NaN
+//     let s  = test_sse2_m128_sin_angle(std::f32::INFINITY);
+//     assert!(s.is_nan());
+
+//     let s  = test_sse2_m128_sin_angle(std::f32::NEG_INFINITY);
+//     assert!(s.is_nan());
+// }
diff --git a/src/f32/quat.rs b/src/f32/quat.rs
index 5906903..15fa7b8 100644
--- a/src/f32/quat.rs
+++ b/src/f32/quat.rs
@@ -351,6 +351,69 @@ impl Quat {
         }
     }
 
+    /// Performs a spherical linear interpolation between `self` and `end`
+    /// based on the value `s`.
+    ///
+    /// When `s` is `0.0`, the result will be equal to `self`.  When `s`
+    /// is `1.0`, the result will be equal to `end`.
+    ///
+    /// Note that a rotation can be represented by two quaternions: `q` and
+    /// `-q`. The slerp path between `q` and `end` will be different from the
+    /// path between `-q` and `end`. One path will take the long way around and
+    /// one will take the short way. In order to correct for this, the `dot`
+    /// product between `self` and `end` should be positive. If the `dot`
+    /// product is negative, slerp between `-self` and `end`.
+    #[inline]
+    pub fn slerp(self, end: Self, s: f32) -> Self {
+        // http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/
+
+        glam_assert!(self.is_normalized());
+        glam_assert!(end.is_normalized());
+
+        const DOT_THRESHOLD: f32 = 0.9995;
+
+        let dot = self.dot(end);
+
+        if dot > DOT_THRESHOLD {
+            // assumes lerp returns a normalized quaternion
+            self.lerp(end, s)
+        } else {
+            #[cfg(vec4f32)]
+            {
+                // assumes scalar_acos clamps the input to [-1.0, 1.0]
+                let theta = crate::f32::funcs::scalar_acos(dot);
+                let scale1 = f32::sin(theta * (1.0 - s));
+                let scale2 = f32::sin(theta * s);
+                let theta_sin = f32::sin(theta);
+
+                Quat((self.0 * scale1 + end.0 * scale2) * theta_sin.recip())
+            }
+
+            #[cfg(vec4sse2)]
+            {
+                // assumes scalar_acos clamps the input to [-1.0, 1.0]
+                let theta = crate::f32::funcs::scalar_acos(dot);
+
+                let x = 1.0 - s;
+                let y = s;
+                let z = 1.0;
+
+                unsafe {
+                    let tmp = Vec4::splat(theta) * Vec4::new(x, y, z, 0.0);
+                    let tmp = crate::f32::funcs::sse2::m128_sin(tmp.0);
+
+                    let scale1 = _mm_shuffle_ps(tmp, tmp, 0b00_00_00_00);
+                    let scale2 = _mm_shuffle_ps(tmp, tmp, 0b01_01_01_01);
+                    let theta_sin = _mm_shuffle_ps(tmp, tmp, 0b10_10_10_10);
+
+                    let theta_sin_recip = Vec4(_mm_rcp_ps(theta_sin));
+
+                    Quat((self.0 * Vec4(scale1) + end.0 * Vec4(scale2)) * theta_sin_recip)
+                }
+            }
+        }
+    }
+
     #[inline]
     /// Multiplies a quaternion and a 3D vector, rotating it.
     pub fn mul_vec3(self, other: Vec3) -> Vec3 {
diff --git a/src/f32/x86_utils.rs b/src/f32/x86_utils.rs
index 166d9dc..47119b4 100644
--- a/src/f32/x86_utils.rs
+++ b/src/f32/x86_utils.rs
@@ -5,6 +5,7 @@ use core::arch::x86_64::*;
 
 #[cfg(any(target_arch = "x86", target_arch = "x86_64"))]
 #[allow(dead_code)]
+#[repr(C)]
 pub(crate) union UnionCast {
     pub m128: __m128,
     pub m128i: __m128i,


Test patch:
diff --git a/tests/quat.rs b/tests/quat.rs
index 99dda65..def0acc 100644
--- a/tests/quat.rs
+++ b/tests/quat.rs
@@ -173,6 +173,31 @@ fn test_quat_lerp() {
     assert_approx_eq!(Quat::from_rotation_y(deg(45.0)), q0.lerp(q1, 0.5));
 }
 
+#[test]
+fn test_quat_slerp() {
+    let q0 = Quat::from_rotation_y(deg(0.0));
+    let q1 = Quat::from_rotation_y(deg(90.0));
+    assert_approx_eq!(q0, q0.slerp(q1, 0.0), 1.0e-3);
+    assert_approx_eq!(q1, q0.slerp(q1, 1.0), 1.0e-3);
+    assert_approx_eq!(Quat::from_rotation_y(deg(45.0)), q0.slerp(q1, 0.5), 1.0e-3);
+}
+
+#[test]
+fn test_quat_slerp_constant_speed() {
+    let step = 0.01;
+    let mut s = 0.0;
+    while s <= 1.0 {
+        let q0 = Quat::from_rotation_y(deg(0.0));
+        let q1 = Quat::from_rotation_y(deg(90.0));
+        assert_approx_eq!(
+            Quat::from_rotation_y(deg(s * 90.0)),
+            q0.slerp(q1, s),
+            1.0e-3
+        );
+        s += step;
+    }
+}
+
 #[test]
 fn test_quat_fmt() {
     let a = Quat::identity();