/* * This file provides: * =================== * * * (Quaternion) * * Index * IndexMut * Quaternion × Quaternion * Quaternion + Quaternion * Quaternion - Quaternion * -Quaternion * Quaternion × Scalar * Quaternion ÷ Scalar * Scalar × Quaternion * * (Unit Quaternion) * UnitQuaternion × UnitQuaternion * UnitQuaternion × Rotation -> UnitQuaternion * Rotation × UnitQuaternion -> UnitQuaternion * * UnitQuaternion ÷ UnitQuaternion * UnitQuaternion ÷ Rotation -> UnitQuaternion * Rotation ÷ UnitQuaternion -> UnitQuaternion * * * UnitQuaternion × Point * UnitQuaternion × Vector * UnitQuaternion × Unit * * NOTE: -UnitQuaternion is already provided by `Unit`. * * * (Assignment Operators) * * Quaternion ×= Scalar * Quaternion ×= Quaternion * Quaternion += Quaternion * Quaternion -= Quaternion * * UnitQuaternion ×= UnitQuaternion * UnitQuaternion ×= Rotation * * UnitQuaternion ÷= UnitQuaternion * UnitQuaternion ÷= Rotation * * TODO: Rotation ×= UnitQuaternion * TODO: Rotation ÷= UnitQuaternion * */ use std::ops::{ Add, AddAssign, Div, DivAssign, Index, IndexMut, Mul, MulAssign, Neg, Sub, SubAssign, }; use crate::base::allocator::Allocator; use crate::base::dimension::{U1, U3, U4}; use crate::base::storage::Storage; use crate::base::{DefaultAllocator, Scalar, Unit, Vector, Vector3}; use crate::SimdRealField; use crate::geometry::{Point3, Quaternion, Rotation, UnitQuaternion}; impl Index for Quaternion { type Output = N; #[inline] fn index(&self, i: usize) -> &Self::Output { &self.coords[i] } } impl IndexMut for Quaternion { #[inline] fn index_mut(&mut self, i: usize) -> &mut N { &mut self.coords[i] } } macro_rules! quaternion_op_impl( ($Op: ident, $op: ident; ($LhsRDim: ident, $LhsCDim: ident), ($RhsRDim: ident, $RhsCDim: ident) $(for $Storage: ident: $StoragesBound: ident $(<$($BoundParam: ty),*>)*),*; $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty, Output = $Result: ty $(=> $VDimA: ty, $VDimB: ty)*; $action: expr; $($lives: tt),*) => { impl<$($lives ,)* N: SimdRealField $(, $Storage: $StoragesBound $(<$($BoundParam),*>)*)*> $Op<$Rhs> for $Lhs where N::Element: SimdRealField, DefaultAllocator: Allocator + Allocator { type Output = $Result; #[inline] fn $op($lhs, $rhs: $Rhs) -> Self::Output { $action } } } ); // Quaternion + Quaternion quaternion_op_impl!( Add, add; (U4, U1), (U4, U1); self: &'a Quaternion, rhs: &'b Quaternion, Output = Quaternion; Quaternion::from(&self.coords + &rhs.coords); 'a, 'b); quaternion_op_impl!( Add, add; (U4, U1), (U4, U1); self: &'a Quaternion, rhs: Quaternion, Output = Quaternion; Quaternion::from(&self.coords + rhs.coords); 'a); quaternion_op_impl!( Add, add; (U4, U1), (U4, U1); self: Quaternion, rhs: &'b Quaternion, Output = Quaternion; Quaternion::from(self.coords + &rhs.coords); 'b); quaternion_op_impl!( Add, add; (U4, U1), (U4, U1); self: Quaternion, rhs: Quaternion, Output = Quaternion; Quaternion::from(self.coords + rhs.coords); ); // Quaternion - Quaternion quaternion_op_impl!( Sub, sub; (U4, U1), (U4, U1); self: &'a Quaternion, rhs: &'b Quaternion, Output = Quaternion; Quaternion::from(&self.coords - &rhs.coords); 'a, 'b); quaternion_op_impl!( Sub, sub; (U4, U1), (U4, U1); self: &'a Quaternion, rhs: Quaternion, Output = Quaternion; Quaternion::from(&self.coords - rhs.coords); 'a); quaternion_op_impl!( Sub, sub; (U4, U1), (U4, U1); self: Quaternion, rhs: &'b Quaternion, Output = Quaternion; Quaternion::from(self.coords - &rhs.coords); 'b); quaternion_op_impl!( Sub, sub; (U4, U1), (U4, U1); self: Quaternion, rhs: Quaternion, Output = Quaternion; Quaternion::from(self.coords - rhs.coords); ); // Quaternion × Quaternion quaternion_op_impl!( Mul, mul; (U4, U1), (U4, U1); self: &'a Quaternion, rhs: &'b Quaternion, Output = Quaternion; Quaternion::new( self[3] * rhs[3] - self[0] * rhs[0] - self[1] * rhs[1] - self[2] * rhs[2], self[3] * rhs[0] + self[0] * rhs[3] + self[1] * rhs[2] - self[2] * rhs[1], self[3] * rhs[1] - self[0] * rhs[2] + self[1] * rhs[3] + self[2] * rhs[0], self[3] * rhs[2] + self[0] * rhs[1] - self[1] * rhs[0] + self[2] * rhs[3]); 'a, 'b); quaternion_op_impl!( Mul, mul; (U4, U1), (U4, U1); self: &'a Quaternion, rhs: Quaternion, Output = Quaternion; self * &rhs; 'a); quaternion_op_impl!( Mul, mul; (U4, U1), (U4, U1); self: Quaternion, rhs: &'b Quaternion, Output = Quaternion; &self * rhs; 'b); quaternion_op_impl!( Mul, mul; (U4, U1), (U4, U1); self: Quaternion, rhs: Quaternion, Output = Quaternion; &self * &rhs; ); // UnitQuaternion × UnitQuaternion quaternion_op_impl!( Mul, mul; (U4, U1), (U4, U1); self: &'a UnitQuaternion, rhs: &'b UnitQuaternion, Output = UnitQuaternion; UnitQuaternion::new_unchecked(self.quaternion() * rhs.quaternion()); 'a, 'b); quaternion_op_impl!( Mul, mul; (U4, U1), (U4, U1); self: &'a UnitQuaternion, rhs: UnitQuaternion, Output = UnitQuaternion; self * &rhs; 'a); quaternion_op_impl!( Mul, mul; (U4, U1), (U4, U1); self: UnitQuaternion, rhs: &'b UnitQuaternion, Output = UnitQuaternion; &self * rhs; 'b); quaternion_op_impl!( Mul, mul; (U4, U1), (U4, U1); self: UnitQuaternion, rhs: UnitQuaternion, Output = UnitQuaternion; &self * &rhs; ); // UnitQuaternion ÷ UnitQuaternion quaternion_op_impl!( Div, div; (U4, U1), (U4, U1); self: &'a UnitQuaternion, rhs: &'b UnitQuaternion, Output = UnitQuaternion; #[allow(clippy::suspicious_arithmetic_impl)] { self * rhs.inverse() }; 'a, 'b); quaternion_op_impl!( Div, div; (U4, U1), (U4, U1); self: &'a UnitQuaternion, rhs: UnitQuaternion, Output = UnitQuaternion; self / &rhs; 'a); quaternion_op_impl!( Div, div; (U4, U1), (U4, U1); self: UnitQuaternion, rhs: &'b UnitQuaternion, Output = UnitQuaternion; &self / rhs; 'b); quaternion_op_impl!( Div, div; (U4, U1), (U4, U1); self: UnitQuaternion, rhs: UnitQuaternion, Output = UnitQuaternion; &self / &rhs; ); // UnitQuaternion × Rotation quaternion_op_impl!( Mul, mul; (U4, U1), (U3, U3); self: &'a UnitQuaternion, rhs: &'b Rotation, Output = UnitQuaternion => U3, U3; // TODO: can we avoid the conversion from a rotation matrix? self * UnitQuaternion::::from_rotation_matrix(rhs); 'a, 'b); quaternion_op_impl!( Mul, mul; (U4, U1), (U3, U3); self: &'a UnitQuaternion, rhs: Rotation, Output = UnitQuaternion => U3, U3; self * UnitQuaternion::::from_rotation_matrix(&rhs); 'a); quaternion_op_impl!( Mul, mul; (U4, U1), (U3, U3); self: UnitQuaternion, rhs: &'b Rotation, Output = UnitQuaternion => U3, U3; self * UnitQuaternion::::from_rotation_matrix(rhs); 'b); quaternion_op_impl!( Mul, mul; (U4, U1), (U3, U3); self: UnitQuaternion, rhs: Rotation, Output = UnitQuaternion => U3, U3; self * UnitQuaternion::::from_rotation_matrix(&rhs); ); // UnitQuaternion ÷ Rotation quaternion_op_impl!( Div, div; (U4, U1), (U3, U3); self: &'a UnitQuaternion, rhs: &'b Rotation, Output = UnitQuaternion => U3, U3; // TODO: can we avoid the conversion to a rotation matrix? self / UnitQuaternion::::from_rotation_matrix(rhs); 'a, 'b); quaternion_op_impl!( Div, div; (U4, U1), (U3, U3); self: &'a UnitQuaternion, rhs: Rotation, Output = UnitQuaternion => U3, U3; self / UnitQuaternion::::from_rotation_matrix(&rhs); 'a); quaternion_op_impl!( Div, div; (U4, U1), (U3, U3); self: UnitQuaternion, rhs: &'b Rotation, Output = UnitQuaternion => U3, U3; self / UnitQuaternion::::from_rotation_matrix(rhs); 'b); quaternion_op_impl!( Div, div; (U4, U1), (U3, U3); self: UnitQuaternion, rhs: Rotation, Output = UnitQuaternion => U3, U3; self / UnitQuaternion::::from_rotation_matrix(&rhs); ); // Rotation × UnitQuaternion quaternion_op_impl!( Mul, mul; (U3, U3), (U4, U1); self: &'a Rotation, rhs: &'b UnitQuaternion, Output = UnitQuaternion => U3, U3; // TODO: can we avoid the conversion from a rotation matrix? UnitQuaternion::::from_rotation_matrix(self) * rhs; 'a, 'b); quaternion_op_impl!( Mul, mul; (U3, U3), (U4, U1); self: &'a Rotation, rhs: UnitQuaternion, Output = UnitQuaternion => U3, U3; UnitQuaternion::::from_rotation_matrix(self) * rhs; 'a); quaternion_op_impl!( Mul, mul; (U3, U3), (U4, U1); self: Rotation, rhs: &'b UnitQuaternion, Output = UnitQuaternion => U3, U3; UnitQuaternion::::from_rotation_matrix(&self) * rhs; 'b); quaternion_op_impl!( Mul, mul; (U3, U3), (U4, U1); self: Rotation, rhs: UnitQuaternion, Output = UnitQuaternion => U3, U3; UnitQuaternion::::from_rotation_matrix(&self) * rhs; ); // Rotation ÷ UnitQuaternion quaternion_op_impl!( Div, div; (U3, U3), (U4, U1); self: &'a Rotation, rhs: &'b UnitQuaternion, Output = UnitQuaternion => U3, U3; // TODO: can we avoid the conversion from a rotation matrix? UnitQuaternion::::from_rotation_matrix(self) / rhs; 'a, 'b); quaternion_op_impl!( Div, div; (U3, U3), (U4, U1); self: &'a Rotation, rhs: UnitQuaternion, Output = UnitQuaternion => U3, U3; UnitQuaternion::::from_rotation_matrix(self) / rhs; 'a); quaternion_op_impl!( Div, div; (U3, U3), (U4, U1); self: Rotation, rhs: &'b UnitQuaternion, Output = UnitQuaternion => U3, U3; UnitQuaternion::::from_rotation_matrix(&self) / rhs; 'b); quaternion_op_impl!( Div, div; (U3, U3), (U4, U1); self: Rotation, rhs: UnitQuaternion, Output = UnitQuaternion => U3, U3; UnitQuaternion::::from_rotation_matrix(&self) / rhs; ); // UnitQuaternion × Vector quaternion_op_impl!( Mul, mul; (U4, U1), (U3, U1) for SB: Storage ; self: &'a UnitQuaternion, rhs: &'b Vector, Output = Vector3 => U3, U4; { let two: N = crate::convert(2.0f64); let t = self.as_ref().vector().cross(rhs) * two; let cross = self.as_ref().vector().cross(&t); t * self.as_ref().scalar() + cross + rhs }; 'a, 'b); quaternion_op_impl!( Mul, mul; (U4, U1), (U3, U1) for SB: Storage ; self: &'a UnitQuaternion, rhs: Vector, Output = Vector3 => U3, U4; self * &rhs; 'a); quaternion_op_impl!( Mul, mul; (U4, U1), (U3, U1) for SB: Storage ; self: UnitQuaternion, rhs: &'b Vector, Output = Vector3 => U3, U4; &self * rhs; 'b); quaternion_op_impl!( Mul, mul; (U4, U1), (U3, U1) for SB: Storage ; self: UnitQuaternion, rhs: Vector, Output = Vector3 => U3, U4; &self * &rhs; ); // UnitQuaternion × Point quaternion_op_impl!( Mul, mul; (U4, U1), (U3, U1); self: &'a UnitQuaternion, rhs: &'b Point3, Output = Point3 => U3, U4; Point3::from(self * &rhs.coords); 'a, 'b); quaternion_op_impl!( Mul, mul; (U4, U1), (U3, U1); self: &'a UnitQuaternion, rhs: Point3, Output = Point3 => U3, U4; Point3::from(self * rhs.coords); 'a); quaternion_op_impl!( Mul, mul; (U4, U1), (U3, U1); self: UnitQuaternion, rhs: &'b Point3, Output = Point3 => U3, U4; Point3::from(self * &rhs.coords); 'b); quaternion_op_impl!( Mul, mul; (U4, U1), (U3, U1); self: UnitQuaternion, rhs: Point3, Output = Point3 => U3, U4; Point3::from(self * rhs.coords); ); // UnitQuaternion × Unit quaternion_op_impl!( Mul, mul; (U4, U1), (U3, U1) for SB: Storage ; self: &'a UnitQuaternion, rhs: &'b Unit>, Output = Unit> => U3, U4; Unit::new_unchecked(self * rhs.as_ref()); 'a, 'b); quaternion_op_impl!( Mul, mul; (U4, U1), (U3, U1) for SB: Storage ; self: &'a UnitQuaternion, rhs: Unit>, Output = Unit> => U3, U4; Unit::new_unchecked(self * rhs.into_inner()); 'a); quaternion_op_impl!( Mul, mul; (U4, U1), (U3, U1) for SB: Storage ; self: UnitQuaternion, rhs: &'b Unit>, Output = Unit> => U3, U4; Unit::new_unchecked(self * rhs.as_ref()); 'b); quaternion_op_impl!( Mul, mul; (U4, U1), (U3, U1) for SB: Storage ; self: UnitQuaternion, rhs: Unit>, Output = Unit> => U3, U4; Unit::new_unchecked(self * rhs.into_inner()); ); macro_rules! scalar_op_impl( ($($Op: ident, $op: ident, $OpAssign: ident, $op_assign: ident);* $(;)*) => {$( impl $Op for Quaternion where N::Element: SimdRealField { type Output = Quaternion; #[inline] fn $op(self, n: N) -> Self::Output { Quaternion::from(self.coords.$op(n)) } } impl<'a, N: SimdRealField> $Op for &'a Quaternion where N::Element: SimdRealField { type Output = Quaternion; #[inline] fn $op(self, n: N) -> Self::Output { Quaternion::from((&self.coords).$op(n)) } } impl $OpAssign for Quaternion where N::Element: SimdRealField { #[inline] fn $op_assign(&mut self, n: N) { self.coords.$op_assign(n) } } )*} ); scalar_op_impl!( Mul, mul, MulAssign, mul_assign; Div, div, DivAssign, div_assign; ); macro_rules! left_scalar_mul_impl( ($($T: ty),* $(,)*) => {$( impl Mul> for $T { type Output = Quaternion<$T>; #[inline] fn mul(self, right: Quaternion<$T>) -> Self::Output { Quaternion::from(self * right.coords) } } impl<'b> Mul<&'b Quaternion<$T>> for $T { type Output = Quaternion<$T>; #[inline] fn mul(self, right: &'b Quaternion<$T>) -> Self::Output { Quaternion::from(self * &right.coords) } } )*} ); left_scalar_mul_impl!(f32, f64); impl Neg for Quaternion where N::Element: SimdRealField, { type Output = Quaternion; #[inline] fn neg(self) -> Self::Output { Self::Output::from(-self.coords) } } impl<'a, N: SimdRealField> Neg for &'a Quaternion where N::Element: SimdRealField, { type Output = Quaternion; #[inline] fn neg(self) -> Self::Output { Self::Output::from(-&self.coords) } } macro_rules! quaternion_op_impl( ($OpAssign: ident, $op_assign: ident; ($LhsRDim: ident, $LhsCDim: ident), ($RhsRDim: ident, $RhsCDim: ident); $lhs: ident: $Lhs: ty, $rhs: ident: $Rhs: ty $(=> $VDimA: ty, $VDimB: ty)*; $action: expr; $($lives: tt),*) => { impl<$($lives ,)* N: SimdRealField> $OpAssign<$Rhs> for $Lhs where N::Element: SimdRealField, DefaultAllocator: Allocator + Allocator { #[inline] fn $op_assign(&mut $lhs, $rhs: $Rhs) { $action } } } ); // Quaternion += Quaternion quaternion_op_impl!( AddAssign, add_assign; (U4, U1), (U4, U1); self: Quaternion, rhs: &'b Quaternion; self.coords += &rhs.coords; 'b); quaternion_op_impl!( AddAssign, add_assign; (U4, U1), (U4, U1); self: Quaternion, rhs: Quaternion; self.coords += rhs.coords; ); // Quaternion -= Quaternion quaternion_op_impl!( SubAssign, sub_assign; (U4, U1), (U4, U1); self: Quaternion, rhs: &'b Quaternion; self.coords -= &rhs.coords; 'b); quaternion_op_impl!( SubAssign, sub_assign; (U4, U1), (U4, U1); self: Quaternion, rhs: Quaternion; self.coords -= rhs.coords; ); // Quaternion ×= Quaternion quaternion_op_impl!( MulAssign, mul_assign; (U4, U1), (U4, U1); self: Quaternion, rhs: &'b Quaternion; { let res = &*self * rhs; // TODO: will this be optimized away? self.coords.copy_from(&res.coords); }; 'b); quaternion_op_impl!( MulAssign, mul_assign; (U4, U1), (U4, U1); self: Quaternion, rhs: Quaternion; *self *= &rhs; ); // UnitQuaternion ×= UnitQuaternion quaternion_op_impl!( MulAssign, mul_assign; (U4, U1), (U4, U1); self: UnitQuaternion, rhs: &'b UnitQuaternion; { let res = &*self * rhs; self.as_mut_unchecked().coords.copy_from(&res.as_ref().coords); }; 'b); quaternion_op_impl!( MulAssign, mul_assign; (U4, U1), (U4, U1); self: UnitQuaternion, rhs: UnitQuaternion; *self *= &rhs; ); // UnitQuaternion ÷= UnitQuaternion quaternion_op_impl!( DivAssign, div_assign; (U4, U1), (U4, U1); self: UnitQuaternion, rhs: &'b UnitQuaternion; { let res = &*self / rhs; self.as_mut_unchecked().coords.copy_from(&res.as_ref().coords); }; 'b); quaternion_op_impl!( DivAssign, div_assign; (U4, U1), (U4, U1); self: UnitQuaternion, rhs: UnitQuaternion; *self /= &rhs; ); // UnitQuaternion ×= Rotation quaternion_op_impl!( MulAssign, mul_assign; (U4, U1), (U3, U3); self: UnitQuaternion, rhs: &'b Rotation => U3, U3; { let res = &*self * rhs; self.as_mut_unchecked().coords.copy_from(&res.as_ref().coords); }; 'b); quaternion_op_impl!( MulAssign, mul_assign; (U4, U1), (U3, U3); self: UnitQuaternion, rhs: Rotation => U3, U3; *self *= &rhs; ); // UnitQuaternion ÷= Rotation quaternion_op_impl!( DivAssign, div_assign; (U4, U1), (U3, U3); self: UnitQuaternion, rhs: &'b Rotation => U3, U3; { let res = &*self / rhs; self.as_mut_unchecked().coords.copy_from(&res.as_ref().coords); }; 'b); quaternion_op_impl!( DivAssign, div_assign; (U4, U1), (U3, U3); self: UnitQuaternion, rhs: Rotation => U3, U3; *self /= &rhs; );