#[cfg(feature = "arbitrary")] use quickcheck::{Arbitrary, Gen}; #[cfg(feature = "arbitrary")] use core::storage::Owned; #[cfg(feature = "arbitrary")] use core::dimension::U4; use rand::{Rand, Rng}; use num::{One, Zero}; use alga::general::Real; use core::{Unit, Vector, Vector3, Vector4}; use core::storage::Storage; use core::dimension::U3; use geometry::{Quaternion, Rotation, UnitQuaternion}; impl Quaternion { /// Creates a quaternion from a 4D vector. The quaternion scalar part corresponds to the `w` /// vector component. #[inline] pub fn from_vector(vector: Vector4) -> Self { Quaternion { coords: vector } } /// Creates a new quaternion from its individual components. Note that the arguments order does /// **not** follow the storage order. /// /// The storage order is `[ x, y, z, w ]`. #[inline] pub fn new(w: N, x: N, y: N, z: N) -> Self { let v = Vector4::::new(x, y, z, w); Self::from_vector(v) } /// Creates a new quaternion from its scalar and vector parts. Note that the arguments order does /// **not** follow the storage order. /// /// The storage order is [ vector, scalar ]. #[inline] // FIXME: take a reference to `vector`? pub fn from_parts(scalar: N, vector: Vector) -> Self where SB: Storage, { Self::new(scalar, vector[0], vector[1], vector[2]) } /// Creates a new quaternion from its polar decomposition. /// /// Note that `axis` is assumed to be a unit vector. // FIXME: take a reference to `axis`? pub fn from_polar_decomposition(scale: N, theta: N, axis: Unit>) -> Self where SB: Storage, { let rot = UnitQuaternion::::from_axis_angle(&axis, theta * ::convert(2.0f64)); rot.unwrap() * scale } /// The quaternion multiplicative identity. #[inline] pub fn identity() -> Self { Self::new(N::one(), N::zero(), N::zero(), N::zero()) } } impl One for Quaternion { #[inline] fn one() -> Self { Self::identity() } } impl Zero for Quaternion { #[inline] fn zero() -> Self { Self::new(N::zero(), N::zero(), N::zero(), N::zero()) } #[inline] fn is_zero(&self) -> bool { self.coords.is_zero() } } impl Rand for Quaternion { #[inline] fn rand(rng: &mut R) -> Self { Quaternion::new(rng.gen(), rng.gen(), rng.gen(), rng.gen()) } } #[cfg(feature = "arbitrary")] impl Arbitrary for Quaternion where Owned: Send, { #[inline] fn arbitrary(g: &mut G) -> Self { Quaternion::new( N::arbitrary(g), N::arbitrary(g), N::arbitrary(g), N::arbitrary(g), ) } } impl UnitQuaternion { /// The quaternion multiplicative identity. #[inline] pub fn identity() -> Self { Self::new_unchecked(Quaternion::identity()) } /// Creates a new quaternion from a unit vector (the rotation axis) and an angle /// (the rotation angle). #[inline] pub fn from_axis_angle(axis: &Unit>, angle: N) -> Self where SB: Storage, { let (sang, cang) = (angle / ::convert(2.0f64)).sin_cos(); let q = Quaternion::from_parts(cang, axis.as_ref() * sang); Self::new_unchecked(q) } /// Creates a new unit quaternion from a quaternion. /// /// The input quaternion will be normalized. #[inline] pub fn from_quaternion(q: Quaternion) -> Self { Self::new_normalize(q) } /// Creates a new unit quaternion from Euler angles. /// /// The primitive rotations are applied in order: 1 roll − 2 pitch − 3 yaw. #[inline] pub fn from_euler_angles(roll: N, pitch: N, yaw: N) -> Self { let (sr, cr) = (roll * ::convert(0.5f64)).sin_cos(); let (sp, cp) = (pitch * ::convert(0.5f64)).sin_cos(); let (sy, cy) = (yaw * ::convert(0.5f64)).sin_cos(); let q = Quaternion::new( cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy, cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy, ); Self::new_unchecked(q) } /// Builds an unit quaternion from a rotation matrix. #[inline] pub fn from_rotation_matrix(rotmat: &Rotation) -> Self { // Robust matrix to quaternion transformation. // See http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion let tr = rotmat[(0, 0)] + rotmat[(1, 1)] + rotmat[(2, 2)]; let res; let _0_25: N = ::convert(0.25); if tr > N::zero() { let denom = (tr + N::one()).sqrt() * ::convert(2.0); res = Quaternion::new( _0_25 * denom, (rotmat[(2, 1)] - rotmat[(1, 2)]) / denom, (rotmat[(0, 2)] - rotmat[(2, 0)]) / denom, (rotmat[(1, 0)] - rotmat[(0, 1)]) / denom, ); } else if rotmat[(0, 0)] > rotmat[(1, 1)] && rotmat[(0, 0)] > rotmat[(2, 2)] { let denom = (N::one() + rotmat[(0, 0)] - rotmat[(1, 1)] - rotmat[(2, 2)]).sqrt() * ::convert(2.0); res = Quaternion::new( (rotmat[(2, 1)] - rotmat[(1, 2)]) / denom, _0_25 * denom, (rotmat[(0, 1)] + rotmat[(1, 0)]) / denom, (rotmat[(0, 2)] + rotmat[(2, 0)]) / denom, ); } else if rotmat[(1, 1)] > rotmat[(2, 2)] { let denom = (N::one() + rotmat[(1, 1)] - rotmat[(0, 0)] - rotmat[(2, 2)]).sqrt() * ::convert(2.0); res = Quaternion::new( (rotmat[(0, 2)] - rotmat[(2, 0)]) / denom, (rotmat[(0, 1)] + rotmat[(1, 0)]) / denom, _0_25 * denom, (rotmat[(1, 2)] + rotmat[(2, 1)]) / denom, ); } else { let denom = (N::one() + rotmat[(2, 2)] - rotmat[(0, 0)] - rotmat[(1, 1)]).sqrt() * ::convert(2.0); res = Quaternion::new( (rotmat[(1, 0)] - rotmat[(0, 1)]) / denom, (rotmat[(0, 2)] + rotmat[(2, 0)]) / denom, (rotmat[(1, 2)] + rotmat[(2, 1)]) / denom, _0_25 * denom, ); } Self::new_unchecked(res) } /// The unit quaternion needed to make `a` and `b` be collinear and point toward the same /// direction. #[inline] pub fn rotation_between(a: &Vector, b: &Vector) -> Option where SB: Storage, SC: Storage, { Self::scaled_rotation_between(a, b, N::one()) } /// The smallest rotation needed to make `a` and `b` collinear and point toward the same /// direction, raised to the power `s`. #[inline] pub fn scaled_rotation_between( a: &Vector, b: &Vector, s: N, ) -> Option where SB: Storage, SC: Storage, { // FIXME: code duplication with Rotation. if let (Some(na), Some(nb)) = ( Unit::try_new(a.clone_owned(), N::zero()), Unit::try_new(b.clone_owned(), N::zero()), ) { Self::scaled_rotation_between_axis(&na, &nb, s) } else { Some(Self::identity()) } } /// The unit quaternion needed to make `a` and `b` be collinear and point toward the same /// direction. #[inline] pub fn rotation_between_axis( a: &Unit>, b: &Unit>, ) -> Option where SB: Storage, SC: Storage, { Self::scaled_rotation_between_axis(a, b, N::one()) } /// The smallest rotation needed to make `a` and `b` collinear and point toward the same /// direction, raised to the power `s`. #[inline] pub fn scaled_rotation_between_axis( na: &Unit>, nb: &Unit>, s: N, ) -> Option where SB: Storage, SC: Storage, { // FIXME: code duplication with Rotation. let c = na.cross(&nb); if let Some(axis) = Unit::try_new(c, N::default_epsilon()) { let cos = na.dot(&nb); // The cosinus may be out of [-1, 1] because of innacuracies. if cos <= -N::one() { return None; } else if cos >= N::one() { return Some(Self::identity()); } else { return Some(Self::from_axis_angle(&axis, cos.acos() * s)); } } else if na.dot(&nb) < N::zero() { // PI // // The rotation axis is undefined but the angle not zero. This is not a // simple rotation. return None; } else { // Zero Some(Self::identity()) } } /// Creates an unit quaternion that corresponds to the local frame of an observer standing at the /// origin and looking toward `dir`. /// /// It maps the view direction `dir` to the positive `z` axis. /// /// # Arguments /// * dir - The look direction, that is, direction the matrix `z` axis will be aligned with. /// * up - The vertical direction. The only requirement of this parameter is to not be /// collinear /// to `dir`. Non-collinearity is not checked. #[inline] pub fn new_observer_frame(dir: &Vector, up: &Vector) -> Self where SB: Storage, SC: Storage, { Self::from_rotation_matrix(&Rotation::::new_observer_frame(dir, up)) } /// Builds a right-handed look-at view matrix without translation. /// /// This conforms to the common notion of right handed look-at matrix from the computer /// graphics community. /// /// # Arguments /// * eye - The eye position. /// * target - The target position. /// * up - A vector approximately aligned with required the vertical axis. The only /// requirement of this parameter is to not be collinear to `target - eye`. #[inline] pub fn look_at_rh(dir: &Vector, up: &Vector) -> Self where SB: Storage, SC: Storage, { Self::new_observer_frame(&-dir, up).inverse() } /// Builds a left-handed look-at view matrix without translation. /// /// This conforms to the common notion of left handed look-at matrix from the computer /// graphics community. /// /// # Arguments /// * eye - The eye position. /// * target - The target position. /// * up - A vector approximately aligned with required the vertical axis. The only /// requirement of this parameter is to not be collinear to `target - eye`. #[inline] pub fn look_at_lh(dir: &Vector, up: &Vector) -> Self where SB: Storage, SC: Storage, { Self::new_observer_frame(dir, up).inverse() } /// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle. /// /// If `axisangle` has a magnitude smaller than `N::default_epsilon()`, this returns the identity rotation. #[inline] pub fn new(axisangle: Vector) -> Self where SB: Storage, { let two: N = ::convert(2.0f64); let q = Quaternion::::from_parts(N::zero(), axisangle / two).exp(); Self::new_unchecked(q) } /// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle. /// /// If `axisangle` has a magnitude smaller than `eps`, this returns the identity rotation. #[inline] pub fn new_eps(axisangle: Vector, eps: N) -> Self where SB: Storage, { let two: N = ::convert(2.0f64); let q = Quaternion::::from_parts(N::zero(), axisangle / two).exp_eps(eps); Self::new_unchecked(q) } /// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle. /// /// If `axisangle` has a magnitude smaller than `N::default_epsilon()`, this returns the identity rotation. /// Same as `Self::new(axisangle)`. #[inline] pub fn from_scaled_axis(axisangle: Vector) -> Self where SB: Storage, { Self::new(axisangle) } /// Creates a new unit quaternion rotation from a rotation axis scaled by the rotation angle. /// /// If `axisangle` has a magnitude smaller than `eps`, this returns the identity rotation. /// Same as `Self::new(axisangle)`. #[inline] pub fn from_scaled_axis_eps(axisangle: Vector, eps: N) -> Self where SB: Storage, { Self::new_eps(axisangle, eps) } } impl One for UnitQuaternion { #[inline] fn one() -> Self { Self::identity() } } impl Rand for UnitQuaternion { #[inline] fn rand(rng: &mut R) -> Self { let axisangle = Vector3::rand(rng); UnitQuaternion::from_scaled_axis(axisangle) } } #[cfg(feature = "arbitrary")] impl Arbitrary for UnitQuaternion where Owned: Send, Owned: Send, { #[inline] fn arbitrary(g: &mut G) -> Self { let axisangle = Vector3::arbitrary(g); UnitQuaternion::from_scaled_axis(axisangle) } }