/* Conversion from quaternions to Euler rotation sequences. From: http://bediyap.com/programming/convert-quaternion-to-euler-rotations/ */ use crate::{DQuat, Quat}; /// Euler rotation sequences. /// /// The angles are applied starting from the right. /// E.g. XYZ will first apply the z-axis rotation. /// /// YXZ can be used for yaw (y-axis), pitch (x-axis), roll (z-axis). #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] pub enum EulerRot { /// Intrinsic three-axis rotation ZYX ZYX, /// Intrinsic three-axis rotation ZXY ZXY, /// Intrinsic three-axis rotation YXZ YXZ, /// Intrinsic three-axis rotation YZX YZX, /// Intrinsic three-axis rotation XYZ XYZ, /// Intrinsic three-axis rotation XZY XZY, } impl Default for EulerRot { /// Default `YXZ` as yaw (y-axis), pitch (x-axis), roll (z-axis). fn default() -> Self { Self::YXZ } } /// Conversion from quaternion to euler angles. pub(crate) trait EulerFromQuaternion: Sized + Copy { type Output; /// Compute the angle of the first axis (X-x-x) fn first(self, q: Q) -> Self::Output; /// Compute then angle of the second axis (x-X-x) fn second(self, q: Q) -> Self::Output; /// Compute then angle of the third axis (x-x-X) fn third(self, q: Q) -> Self::Output; /// Compute all angles of a rotation in the notation order fn convert_quat(self, q: Q) -> (Self::Output, Self::Output, Self::Output); #[doc(hidden)] fn sine_theta(self, q: Q) -> Self::Output; } /// Conversion from euler angles to quaternion. pub(crate) trait EulerToQuaternion: Copy { type Output; /// Create the rotation quaternion for the three angles of this euler rotation sequence. fn new_quat(self, u: T, v: T, w: T) -> Self::Output; } macro_rules! impl_from_quat { ($t:ident, $quat:ident) => { impl EulerFromQuaternion<$quat> for EulerRot { type Output = $t; fn sine_theta(self, q: $quat) -> $t { use EulerRot::*; match self { ZYX => -2.0 * (q.x * q.z - q.w * q.y), ZXY => 2.0 * (q.y * q.z + q.w * q.x), YXZ => -2.0 * (q.y * q.z - q.w * q.x), YZX => 2.0 * (q.x * q.y + q.w * q.z), XYZ => 2.0 * (q.x * q.z + q.w * q.y), XZY => -2.0 * (q.x * q.y - q.w * q.z), } .clamp(-1.0, 1.0) } fn first(self, q: $quat) -> $t { use crate::$t::math; use EulerRot::*; let sine_theta = self.sine_theta(q); if math::abs(sine_theta) > 0.99999 { let scale = 2.0 * math::signum(sine_theta); match self { ZYX => scale * math::atan2(-q.x, q.w), ZXY => scale * math::atan2(q.y, q.w), YXZ => scale * math::atan2(-q.z, q.w), YZX => scale * math::atan2(q.x, q.w), XYZ => scale * math::atan2(q.z, q.w), XZY => scale * math::atan2(-q.y, q.w), } } else { match self { ZYX => math::atan2( 2.0 * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z, ), ZXY => math::atan2( -2.0 * (q.x * q.y - q.w * q.z), q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z, ), YXZ => math::atan2( 2.0 * (q.x * q.z + q.w * q.y), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z, ), YZX => math::atan2( -2.0 * (q.x * q.z - q.w * q.y), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z, ), XYZ => math::atan2( -2.0 * (q.y * q.z - q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z, ), XZY => math::atan2( 2.0 * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z, ), } } } fn second(self, q: $quat) -> $t { use crate::$t::math; math::asin(self.sine_theta(q)) } fn third(self, q: $quat) -> $t { use crate::$t::math; use EulerRot::*; if math::abs(self.sine_theta(q)) > 0.99999 { 0.0 } else { match self { ZYX => math::atan2( 2.0 * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z, ), ZXY => math::atan2( -2.0 * (q.x * q.z - q.w * q.y), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z, ), YXZ => math::atan2( 2.0 * (q.x * q.y + q.w * q.z), q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z, ), YZX => math::atan2( -2.0 * (q.y * q.z - q.w * q.x), q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z, ), XYZ => math::atan2( -2.0 * (q.x * q.y - q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z, ), XZY => math::atan2( 2.0 * (q.x * q.z + q.w * q.y), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z, ), } } } fn convert_quat(self, q: $quat) -> ($t, $t, $t) { use crate::$t::math; use EulerRot::*; let sine_theta = self.sine_theta(q); let second = math::asin(sine_theta); if math::abs(sine_theta) > 0.99999 { let scale = 2.0 * math::signum(sine_theta); return match self { ZYX => (scale * math::atan2(-q.x, q.w), second, 0.0), ZXY => (scale * math::atan2(q.y, q.w), second, 0.0), YXZ => (scale * math::atan2(-q.z, q.w), second, 0.0), YZX => (scale * math::atan2(q.x, q.w), second, 0.0), XYZ => (scale * math::atan2(q.z, q.w), second, 0.0), XZY => (scale * math::atan2(-q.y, q.w), second, 0.0), }; } let first = match self { ZYX => math::atan2( 2.0 * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z, ), ZXY => math::atan2( -2.0 * (q.x * q.y - q.w * q.z), q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z, ), YXZ => math::atan2( 2.0 * (q.x * q.z + q.w * q.y), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z, ), YZX => math::atan2( -2.0 * (q.x * q.z - q.w * q.y), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z, ), XYZ => math::atan2( -2.0 * (q.y * q.z - q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z, ), XZY => math::atan2( 2.0 * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z, ), }; let third = match self { ZYX => math::atan2( 2.0 * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z, ), ZXY => math::atan2( -2.0 * (q.x * q.z - q.w * q.y), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z, ), YXZ => math::atan2( 2.0 * (q.x * q.y + q.w * q.z), q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z, ), YZX => math::atan2( -2.0 * (q.y * q.z - q.w * q.x), q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z, ), XYZ => math::atan2( -2.0 * (q.x * q.y - q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z, ), XZY => math::atan2( 2.0 * (q.x * q.z + q.w * q.y), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z, ), }; (first, second, third) } } // End - impl EulerFromQuaternion }; } macro_rules! impl_to_quat { ($t:ty, $quat:ident) => { impl EulerToQuaternion<$t> for EulerRot { type Output = $quat; #[inline(always)] fn new_quat(self, u: $t, v: $t, w: $t) -> $quat { use EulerRot::*; #[inline(always)] fn rot_x(a: $t) -> $quat { $quat::from_rotation_x(a) } #[inline(always)] fn rot_y(a: $t) -> $quat { $quat::from_rotation_y(a) } #[inline(always)] fn rot_z(a: $t) -> $quat { $quat::from_rotation_z(a) } match self { ZYX => rot_z(u) * rot_y(v) * rot_x(w), ZXY => rot_z(u) * rot_x(v) * rot_y(w), YXZ => rot_y(u) * rot_x(v) * rot_z(w), YZX => rot_y(u) * rot_z(v) * rot_x(w), XYZ => rot_x(u) * rot_y(v) * rot_z(w), XZY => rot_x(u) * rot_z(v) * rot_y(w), } .normalize() } } // End - impl EulerToQuaternion }; } impl_from_quat!(f32, Quat); impl_from_quat!(f64, DQuat); impl_to_quat!(f32, Quat); impl_to_quat!(f64, DQuat);