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 { pub v: Vector3, } +#[cfg(feature = "use_simd")] +impl From for Quaternion { + #[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 for Quaternion { + #[inline] + fn into(self) -> Simdf32x4 { + let self_ref: &[f32; 4] = self.as_ref(); + Simdf32x4::load(self_ref.as_ref(), 0 as usize) + } +} + impl Quaternion { /// Construct a new quaternion from one scalar component and three /// imaginary components @@ -73,7 +99,7 @@ impl Quaternion { let mag_avg = (src.magnitude2() * dst.magnitude2()).sqrt(); let dot = src.dot(dst); if ulps_eq!(dot, &mag_avg) { - Quaternion::one() + Quaternion::::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 Zero for Quaternion { #[inline] fn is_zero(&self) -> bool { - ulps_eq!(self, &Quaternion::zero()) + ulps_eq!(self, &Quaternion::::zero()) } } @@ -175,6 +201,7 @@ impl MetricSpace for Quaternion { } } +#[cfg(not(feature = "use_simd"))] impl InnerSpace for Quaternion { #[inline] fn dot(self, other: Quaternion) -> S { @@ -182,6 +209,25 @@ impl InnerSpace for Quaternion { } } +#[cfg(feature = "use_simd")] +impl InnerSpace for Quaternion { + #[inline] + default fn dot(self, other: Quaternion) -> S { + self.s * other.s + self.v.dot(other.v) + } +} + +#[cfg(feature = "use_simd")] +impl InnerSpace for Quaternion { + #[inline] + fn dot(self, other: Quaternion) -> 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 From> for Quaternion<::Unitless> where A: Angle + Into::Unitless>>, { @@ -203,35 +249,119 @@ impl From> for Quaternion<::Unitless> where } } +#[cfg(not(feature = "use_simd"))] impl_operator!( Neg for Quaternion { fn neg(quat) -> Quaternion { Quaternion::from_sv(-quat.s, -quat.v) } }); +#[cfg(feature = "use_simd")] +impl_operator_default!( Neg for Quaternion { + fn neg(quat) -> Quaternion { + Quaternion::from_sv(-quat.s, -quat.v) + } +}); + +#[cfg(feature = "use_simd")] +impl_operator_simd!{ + [Simdf32x4]; Neg for Quaternion { + fn neg(lhs) -> Quaternion { + (-lhs).into() + } + } +} + +#[cfg(not(feature = "use_simd"))] impl_operator!( Mul for Quaternion { fn mul(lhs, rhs) -> Quaternion { Quaternion::from_sv(lhs.s * rhs, lhs.v * rhs) } }); + +#[cfg(feature = "use_simd")] +impl_operator_default!( Mul for Quaternion { + fn mul(lhs, rhs) -> Quaternion { + Quaternion::from_sv(lhs.s * rhs, lhs.v * rhs) + } +}); + +#[cfg(feature = "use_simd")] +impl_operator_simd!{@rs + [Simdf32x4]; Mul for Quaternion { + fn mul(lhs, rhs) -> Quaternion { + (lhs * rhs).into() + } + } +} + +#[cfg(not(feature = "use_simd"))] impl_assignment_operator!( MulAssign for Quaternion { fn mul_assign(&mut self, scalar) { self.s *= scalar; self.v *= scalar; } }); +#[cfg(feature = "use_simd")] +impl_assignment_operator_default!( MulAssign for Quaternion { + fn mul_assign(&mut self, scalar) { self.s *= scalar; self.v *= scalar; } +}); + +#[cfg(feature = "use_simd")] +impl MulAssign for Quaternion { + 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!( Div for Quaternion { fn div(lhs, rhs) -> Quaternion { Quaternion::from_sv(lhs.s / rhs, lhs.v / rhs) } }); + +#[cfg(feature = "use_simd")] +impl_operator_default!( Div for Quaternion { + fn div(lhs, rhs) -> Quaternion { + Quaternion::from_sv(lhs.s / rhs, lhs.v / rhs) + } +}); + +#[cfg(feature = "use_simd")] +impl_operator_simd!{@rs + [Simdf32x4]; Div for Quaternion { + fn div(lhs, rhs) -> Quaternion { + (lhs / rhs).into() + } + } +} + +#[cfg(not(feature = "use_simd"))] impl_assignment_operator!( DivAssign for Quaternion { fn div_assign(&mut self, scalar) { self.s /= scalar; self.v /= scalar; } }); +#[cfg(feature = "use_simd")] +impl_assignment_operator_default!( DivAssign for Quaternion { + fn div_assign(&mut self, scalar) { self.s /= scalar; self.v /= scalar; } +}); + +#[cfg(feature = "use_simd")] +impl DivAssign for Quaternion { + fn div_assign(&mut self, other: f32) { + let s: Simdf32x4 = (*self).into(); + let other = Simdf32x4::splat(other); + *self = (s / other).into(); + } +} + impl_operator!( Rem for Quaternion { fn rem(lhs, rhs) -> Quaternion { Quaternion::from_sv(lhs.s % rhs, lhs.v % rhs) } }); + impl_assignment_operator!( RemAssign for Quaternion { fn rem_assign(&mut self, scalar) { self.s %= scalar; self.v %= scalar; } }); @@ -245,24 +375,93 @@ impl_operator!( Mul > for Quaternion { }} }); +#[cfg(not(feature = "use_simd"))] impl_operator!( Add > for Quaternion { fn add(lhs, rhs) -> Quaternion { Quaternion::from_sv(lhs.s + rhs.s, lhs.v + rhs.v) } }); + +#[cfg(feature = "use_simd")] +impl_operator_default!( Add > for Quaternion { + fn add(lhs, rhs) -> Quaternion { + Quaternion::from_sv(lhs.s + rhs.s, lhs.v + rhs.v) + } +}); + +#[cfg(feature = "use_simd")] +impl_operator_simd!{ + [Simdf32x4]; Add> for Quaternion { + fn add(lhs, rhs) -> Quaternion { + (lhs + rhs).into() + } + } +} + +#[cfg(not(feature = "use_simd"))] impl_assignment_operator!( AddAssign > for Quaternion { fn add_assign(&mut self, other) { self.s += other.s; self.v += other.v; } }); +#[cfg(feature = "use_simd")] +impl_assignment_operator_default!( AddAssign > for Quaternion { + fn add_assign(&mut self, other) { self.s += other.s; self.v += other.v; } +}); + +#[cfg(feature = "use_simd")] +impl AddAssign for Quaternion { + #[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!( Sub > for Quaternion { fn sub(lhs, rhs) -> Quaternion { Quaternion::from_sv(lhs.s - rhs.s, lhs.v - rhs.v) } }); + +#[cfg(feature = "use_simd")] +impl_operator_default!( Sub > for Quaternion { + fn sub(lhs, rhs) -> Quaternion { + Quaternion::from_sv(lhs.s - rhs.s, lhs.v - rhs.v) + } +}); + +#[cfg(feature = "use_simd")] +impl_operator_simd!{ + [Simdf32x4]; Sub> for Quaternion { + fn sub(lhs, rhs) -> Quaternion { + (lhs - rhs).into() + } + } +} + +#[cfg(not(feature = "use_simd"))] impl_assignment_operator!( SubAssign > for Quaternion { fn sub_assign(&mut self, other) { self.s -= other.s; self.v -= other.v; } }); +#[cfg(feature = "use_simd")] +impl_assignment_operator_default!( SubAssign > for Quaternion { + fn sub_assign(&mut self, other) { self.s -= other.s; self.v -= other.v; } +}); + +#[cfg(feature = "use_simd")] +impl SubAssign for Quaternion { + #[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!( Mul > for Quaternion { fn mul(lhs, rhs) -> Quaternion { 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!( Mul > for Quaternion { } }); +#[cfg(feature = "use_simd")] +impl_operator_default!( Mul > for Quaternion { + fn mul(lhs, rhs) -> Quaternion { + 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> for Quaternion { + fn mul(lhs, rhs) -> Quaternion { + { + 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> 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 for Vector4 { } } +#[cfg(feature = "use_simd")] +impl_operator_simd!{ + [Simdf32x4]; Add> for Vector4 { + fn add(lhs, rhs) -> Vector4 { + (lhs + rhs).into() + } + } +} #[cfg(feature = "use_simd")] impl_operator_simd!{ @@ -1050,7 +1058,6 @@ impl Into for Vector4 { } } - #[cfg(feature = "use_simd")] impl_operator_simd!{ [Simdu32x4]; Add> for Vector4 { @@ -1060,15 +1067,6 @@ impl_operator_simd!{ } } -#[cfg(feature = "use_simd")] -impl_operator_simd!{ - [Simdf32x4]; Add> for Vector4 { - fn add(lhs, rhs) -> Vector4 { - (lhs + rhs).into() - } - } -} - #[cfg(feature = "use_simd")] impl_operator_simd!{ [Simdu32x4]; Sub> for Vector4 {