From 194c4770e968ed4c10fc326c6d9ec541b42a901a Mon Sep 17 00:00:00 2001
From: Luxko <liarluxlux@gmail.com>
Date: Sun, 12 Mar 2017 20:44:58 +0800
Subject: [PATCH] add basic SIMD support for Quaternion<f32>

---
 src/quaternion.rs | 234 +++++++++++++++++++++++++++++++++++++++++++++-
 src/vector.rs     |  18 ++--
 2 files changed, 240 insertions(+), 12 deletions(-)

diff --git a/src/quaternion.rs b/src/quaternion.rs
index c97aea6..384bf1e 100644
--- a/src/quaternion.rs
+++ b/src/quaternion.rs
@@ -30,6 +30,8 @@ use point::Point3;
 use rotation::{Rotation, Rotation3, Basis3};
 use vector::Vector3;
 
+#[cfg(feature = "use_simd")]
+use simd::f32x4 as Simdf32x4;
 
 /// A [quaternion](https://en.wikipedia.org/wiki/Quaternion) in scalar/vector
 /// form.
@@ -46,6 +48,30 @@ pub struct Quaternion<S> {
     pub v: Vector3<S>,
 }
 
+#[cfg(feature = "use_simd")]
+impl From<Simdf32x4> for Quaternion<f32> {
+    #[inline]
+    fn from(f: Simdf32x4) -> Self {
+        unsafe {
+            let mut ret: Self = mem::uninitialized();
+            {
+                let ret_mut: &mut [f32; 4] = ret.as_mut();
+                f.store(ret_mut.as_mut(), 0 as usize);
+            }
+            ret
+        }
+    }
+}
+
+#[cfg(feature = "use_simd")]
+impl Into<Simdf32x4> for Quaternion<f32> {
+    #[inline]
+    fn into(self) -> Simdf32x4 {
+        let self_ref: &[f32; 4] = self.as_ref();
+        Simdf32x4::load(self_ref.as_ref(), 0 as usize)
+    }
+}
+
 impl<S: BaseFloat> Quaternion<S> {
     /// Construct a new quaternion from one scalar component and three
     /// imaginary components
@@ -73,7 +99,7 @@ impl<S: BaseFloat> Quaternion<S> {
         let mag_avg = (src.magnitude2() * dst.magnitude2()).sqrt();
         let dot = src.dot(dst);
         if ulps_eq!(dot, &mag_avg) {
-            Quaternion::one()
+            Quaternion::<S>::one()
         } else if ulps_eq!(dot, &-mag_avg) {
             let axis = fallback.unwrap_or_else(|| {
                 let mut v = Vector3::unit_x().cross(src);
@@ -151,7 +177,7 @@ impl<S: BaseFloat> Zero for Quaternion<S> {
 
     #[inline]
     fn is_zero(&self) -> bool {
-        ulps_eq!(self, &Quaternion::zero())
+        ulps_eq!(self, &Quaternion::<S>::zero())
     }
 }
 
@@ -175,6 +201,7 @@ impl<S: BaseFloat> MetricSpace for Quaternion<S> {
     }
 }
 
+#[cfg(not(feature = "use_simd"))]
 impl<S: BaseFloat> InnerSpace for Quaternion<S> {
     #[inline]
     fn dot(self, other: Quaternion<S>) -> S {
@@ -182,6 +209,25 @@ impl<S: BaseFloat> InnerSpace for Quaternion<S> {
     }
 }
 
+#[cfg(feature = "use_simd")]
+impl<S: BaseFloat> InnerSpace for Quaternion<S> {
+    #[inline]
+    default fn dot(self, other: Quaternion<S>) -> S {
+        self.s * other.s + self.v.dot(other.v)
+    }
+}
+
+#[cfg(feature = "use_simd")]
+impl InnerSpace for Quaternion<f32> {
+    #[inline]
+    fn dot(self, other: Quaternion<f32>) -> f32 {
+        let lhs: Simdf32x4 = self.into();
+        let rhs: Simdf32x4 = other.into();
+        let r = lhs * rhs;
+        r.extract(0) + r.extract(1) + r.extract(2) + r.extract(3)
+    }
+}
+
 impl<A> From<Euler<A>> for Quaternion<<A as Angle>::Unitless> where
     A: Angle + Into<Rad<<A as Angle>::Unitless>>,
 {
@@ -203,35 +249,119 @@ impl<A> From<Euler<A>> for Quaternion<<A as Angle>::Unitless> where
     }
 }
 
+#[cfg(not(feature = "use_simd"))]
 impl_operator!(<S: BaseFloat> Neg for Quaternion<S> {
     fn neg(quat) -> Quaternion<S> {
         Quaternion::from_sv(-quat.s, -quat.v)
     }
 });
 
+#[cfg(feature = "use_simd")]
+impl_operator_default!(<S: BaseFloat> Neg for Quaternion<S> {
+    fn neg(quat) -> Quaternion<S> {
+        Quaternion::from_sv(-quat.s, -quat.v)
+    }
+});
+
+#[cfg(feature = "use_simd")]
+impl_operator_simd!{
+    [Simdf32x4]; Neg for Quaternion<f32> {
+        fn neg(lhs) -> Quaternion<f32> {
+            (-lhs).into()
+        }
+    }
+}
+
+#[cfg(not(feature = "use_simd"))]
 impl_operator!(<S: BaseFloat> Mul<S> for Quaternion<S> {
     fn mul(lhs, rhs) -> Quaternion<S> {
         Quaternion::from_sv(lhs.s * rhs, lhs.v * rhs)
     }
 });
+
+#[cfg(feature = "use_simd")]
+impl_operator_default!(<S: BaseFloat> Mul<S> for Quaternion<S> {
+    fn mul(lhs, rhs) -> Quaternion<S> {
+        Quaternion::from_sv(lhs.s * rhs, lhs.v * rhs)
+    }
+});
+
+#[cfg(feature = "use_simd")]
+impl_operator_simd!{@rs
+    [Simdf32x4]; Mul<f32> for Quaternion<f32> {
+        fn mul(lhs, rhs) -> Quaternion<f32> {
+            (lhs * rhs).into()
+        }
+    }
+}
+
+#[cfg(not(feature = "use_simd"))]
 impl_assignment_operator!(<S: BaseFloat> MulAssign<S> for Quaternion<S> {
     fn mul_assign(&mut self, scalar) { self.s *= scalar; self.v *= scalar; }
 });
 
+#[cfg(feature = "use_simd")]
+impl_assignment_operator_default!(<S: BaseFloat> MulAssign<S> for Quaternion<S> {
+    fn mul_assign(&mut self, scalar) { self.s *= scalar; self.v *= scalar; }
+});
+
+#[cfg(feature = "use_simd")]
+impl MulAssign<f32> for Quaternion<f32> {
+    fn mul_assign(&mut self, other: f32) {
+        let s: Simdf32x4 = (*self).into();
+        let other = Simdf32x4::splat(other);
+        *self = (s * other).into();
+    }
+}
+
+#[cfg(not(feature = "use_simd"))]
 impl_operator!(<S: BaseFloat> Div<S> for Quaternion<S> {
     fn div(lhs, rhs) -> Quaternion<S> {
         Quaternion::from_sv(lhs.s / rhs, lhs.v / rhs)
     }
 });
+
+#[cfg(feature = "use_simd")]
+impl_operator_default!(<S: BaseFloat> Div<S> for Quaternion<S> {
+    fn div(lhs, rhs) -> Quaternion<S> {
+        Quaternion::from_sv(lhs.s / rhs, lhs.v / rhs)
+    }
+});
+
+#[cfg(feature = "use_simd")]
+impl_operator_simd!{@rs
+    [Simdf32x4]; Div<f32> for Quaternion<f32> {
+        fn div(lhs, rhs) -> Quaternion<f32> {
+            (lhs / rhs).into()
+        }
+    }
+}
+
+#[cfg(not(feature = "use_simd"))]
 impl_assignment_operator!(<S: BaseFloat> DivAssign<S> for Quaternion<S> {
     fn div_assign(&mut self, scalar) { self.s /= scalar; self.v /= scalar; }
 });
 
+#[cfg(feature = "use_simd")]
+impl_assignment_operator_default!(<S: BaseFloat> DivAssign<S> for Quaternion<S> {
+    fn div_assign(&mut self, scalar) { self.s /= scalar; self.v /= scalar; }
+});
+
+#[cfg(feature = "use_simd")]
+impl DivAssign<f32> for Quaternion<f32> {
+    fn div_assign(&mut self, other: f32) {
+        let s: Simdf32x4 = (*self).into();
+        let other = Simdf32x4::splat(other);
+        *self = (s / other).into();
+    }
+}
+
 impl_operator!(<S: BaseFloat> Rem<S> for Quaternion<S> {
     fn rem(lhs, rhs) -> Quaternion<S> {
         Quaternion::from_sv(lhs.s % rhs, lhs.v % rhs)
     }
 });
+
 impl_assignment_operator!(<S: BaseFloat> RemAssign<S> for Quaternion<S> {
     fn rem_assign(&mut self, scalar) { self.s %= scalar; self.v %= scalar; }
 });
@@ -245,24 +375,93 @@ impl_operator!(<S: BaseFloat> Mul<Vector3<S> > for Quaternion<S> {
     }}
 });
 
+#[cfg(not(feature = "use_simd"))]
 impl_operator!(<S: BaseFloat> Add<Quaternion<S> > for Quaternion<S> {
     fn add(lhs, rhs) -> Quaternion<S> {
         Quaternion::from_sv(lhs.s + rhs.s, lhs.v + rhs.v)
     }
 });
+
+#[cfg(feature = "use_simd")]
+impl_operator_default!(<S: BaseFloat> Add<Quaternion<S> > for Quaternion<S> {
+    fn add(lhs, rhs) -> Quaternion<S> {
+        Quaternion::from_sv(lhs.s + rhs.s, lhs.v + rhs.v)
+    }
+});
+
+#[cfg(feature = "use_simd")]
+impl_operator_simd!{
+    [Simdf32x4]; Add<Quaternion<f32>> for Quaternion<f32> {
+        fn add(lhs, rhs) -> Quaternion<f32> {
+            (lhs + rhs).into()
+        }
+    }
+}
+
+#[cfg(not(feature = "use_simd"))]
 impl_assignment_operator!(<S: BaseFloat> AddAssign<Quaternion<S> > for Quaternion<S> {
     fn add_assign(&mut self, other) { self.s += other.s; self.v += other.v; }
 });
 
+#[cfg(feature = "use_simd")]
+impl_assignment_operator_default!(<S: BaseFloat> AddAssign<Quaternion<S> > for Quaternion<S> {
+    fn add_assign(&mut self, other) { self.s += other.s; self.v += other.v; }
+});
+
+#[cfg(feature = "use_simd")]
+impl AddAssign for Quaternion<f32> {
+    #[inline]
+    fn add_assign(&mut self, rhs: Self) {
+        let s: Simdf32x4 = (*self).into();
+        let rhs: Simdf32x4 = rhs.into();
+        *self = (s + rhs).into();
+    }
+}
+
+#[cfg(not(feature = "use_simd"))]
 impl_operator!(<S: BaseFloat> Sub<Quaternion<S> > for Quaternion<S> {
     fn sub(lhs, rhs) -> Quaternion<S> {
         Quaternion::from_sv(lhs.s - rhs.s, lhs.v - rhs.v)
     }
 });
+
+#[cfg(feature = "use_simd")]
+impl_operator_default!(<S: BaseFloat> Sub<Quaternion<S> > for Quaternion<S> {
+    fn sub(lhs, rhs) -> Quaternion<S> {
+        Quaternion::from_sv(lhs.s - rhs.s, lhs.v - rhs.v)
+    }
+});
+
+#[cfg(feature = "use_simd")]
+impl_operator_simd!{
+    [Simdf32x4]; Sub<Quaternion<f32>> for Quaternion<f32> {
+        fn sub(lhs, rhs) -> Quaternion<f32> {
+            (lhs - rhs).into()
+        }
+    }
+}
+
+#[cfg(not(feature = "use_simd"))]
 impl_assignment_operator!(<S: BaseFloat> SubAssign<Quaternion<S> > for Quaternion<S> {
     fn sub_assign(&mut self, other) { self.s -= other.s; self.v -= other.v; }
 });
 
+#[cfg(feature = "use_simd")]
+impl_assignment_operator_default!(<S: BaseFloat> SubAssign<Quaternion<S> > for Quaternion<S> {
+    fn sub_assign(&mut self, other) { self.s -= other.s; self.v -= other.v; }
+});
+
+#[cfg(feature = "use_simd")]
+impl SubAssign for Quaternion<f32> {
+    #[inline]
+    fn sub_assign(&mut self, rhs: Self) {
+        let s: Simdf32x4 = (*self).into();
+        let rhs: Simdf32x4 = rhs.into();
+        *self = (s - rhs).into();
+    }
+}
+
+#[cfg(not(feature = "use_simd"))]
 impl_operator!(<S: BaseFloat> Mul<Quaternion<S> > for Quaternion<S> {
     fn mul(lhs, rhs) -> Quaternion<S> {
         Quaternion::new(lhs.s * rhs.s - lhs.v.x * rhs.v.x - lhs.v.y * rhs.v.y - lhs.v.z * rhs.v.z,
@@ -272,6 +471,37 @@ impl_operator!(<S: BaseFloat> Mul<Quaternion<S> > for Quaternion<S> {
     }
 });
 
+#[cfg(feature = "use_simd")]
+impl_operator_default!(<S: BaseFloat> Mul<Quaternion<S> > for Quaternion<S> {
+    fn mul(lhs, rhs) -> Quaternion<S> {
+        Quaternion::new(lhs.s * rhs.s - lhs.v.x * rhs.v.x - lhs.v.y * rhs.v.y - lhs.v.z * rhs.v.z,
+                        lhs.s * rhs.v.x + lhs.v.x * rhs.s + lhs.v.y * rhs.v.z - lhs.v.z * rhs.v.y,
+                        lhs.s * rhs.v.y + lhs.v.y * rhs.s + lhs.v.z * rhs.v.x - lhs.v.x * rhs.v.z,
+                        lhs.s * rhs.v.z + lhs.v.z * rhs.s + lhs.v.x * rhs.v.y - lhs.v.y * rhs.v.x)
+    }
+});
+
+#[cfg(feature = "use_simd")]
+impl_operator_simd!{
+    [Simdf32x4]; Mul<Quaternion<f32>> for Quaternion<f32> {
+        fn mul(lhs, rhs) -> Quaternion<f32> {
+            {
+                let p0 = Simdf32x4::splat(lhs.extract(0)) * rhs;
+                let p1 = Simdf32x4::splat(lhs.extract(1)) * Simdf32x4::new(
+                    -rhs.extract(1), rhs.extract(0), -rhs.extract(3), rhs.extract(2)
+                );
+                let p2 = Simdf32x4::splat(lhs.extract(2)) * Simdf32x4::new(
+                    -rhs.extract(2), rhs.extract(3), rhs.extract(0), -rhs.extract(1)
+                );
+                let p3 = Simdf32x4::splat(lhs.extract(3)) * Simdf32x4::new(
+                    -rhs.extract(3), -rhs.extract(2), rhs.extract(1), rhs.extract(0)
+                );
+                (p0 + p1 + p2 + p3).into()
+            }
+        }
+    }
+}
+
 macro_rules! impl_scalar_mul {
     ($S:ident) => {
         impl_operator!(Mul<Quaternion<$S>> for $S {
diff --git a/src/vector.rs b/src/vector.rs
index f3828a6..a289614 100644
--- a/src/vector.rs
+++ b/src/vector.rs
@@ -802,6 +802,14 @@ impl Into<Simdf32x4> for Vector4<f32> {
     }
 }
 
+#[cfg(feature = "use_simd")]
+impl_operator_simd!{
+    [Simdf32x4]; Add<Vector4<f32>> for Vector4<f32> {
+        fn add(lhs, rhs) -> Vector4<f32> {
+            (lhs + rhs).into()
+        }
+    }
+}
 
 #[cfg(feature = "use_simd")]
 impl_operator_simd!{
@@ -1050,7 +1058,6 @@ impl Into<Simdu32x4> for Vector4<u32> {
     }
 }
 
-
 #[cfg(feature = "use_simd")]
 impl_operator_simd!{
     [Simdu32x4]; Add<Vector4<u32>> for Vector4<u32> {
@@ -1060,15 +1067,6 @@ impl_operator_simd!{
     }
 }
 
-#[cfg(feature = "use_simd")]
-impl_operator_simd!{
-    [Simdf32x4]; Add<Vector4<f32>> for Vector4<f32> {
-        fn add(lhs, rhs) -> Vector4<f32> {
-            (lhs + rhs).into()
-        }
-    }
-}
-
 #[cfg(feature = "use_simd")]
 impl_operator_simd!{
     [Simdu32x4]; Sub<Vector4<u32>> for Vector4<u32> {