diff --git a/src/quaternion.rs b/src/quaternion.rs index 893dfe2..e6d5b26 100644 --- a/src/quaternion.rs +++ b/src/quaternion.rs @@ -17,7 +17,7 @@ use std::fmt; use std::mem; use std::num::{zero, one, cast}; -use angle::{Angle, Rad, acos, sin, sin_cos}; +use angle::{Angle, Rad, acos, sin, sin_cos, rad}; use approx::ApproxEq; use array::Array1; use matrix::{Matrix3, ToMatrix3, ToMatrix4, Matrix4}; @@ -263,6 +263,45 @@ impl Quaternion { .mul_s(sin(theta).recip()) } } + + /// Convert a Quaternion to Eular angles + /// This is a polar singularity aware conversion + /// + /// Based on: + /// - [Maths - Conversion Quaternion to Euler] + /// (http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/) + pub fn to_euler(&self) -> (Rad, Rad, Rad) { + let sig: S = cast(0.499f64).unwrap(); + let two: S = cast(2f64).unwrap(); + let one: S = cast(1f64).unwrap(); + + let (qw, qx, qy, qz) = (self.s, self.v.x, self.v.y, self.v.z); + let (sqw, sqx, sqy, sqz) = (qw*qw, qx*qx, qy*qy, qz*qz); + + let unit = sqx + sqy + sqz + sqw; + let test = qx*qy + qz*qw; + + if test > sig * unit { + ( + rad(zero::()), + rad(Float::frac_pi_2()), + rad(two * qx.atan2(qw)), + ) + } else if test < -sig * unit { + let y: S = Float::frac_pi_2(); + ( + rad(zero::()), + rad(-y), + rad(two * qx.atan2(qw)), + ) + } else { + ( + rad((two * (qy*qw - qx*qz)).atan2(one - two*(sqy + sqz))), + rad((two * (qx*qy + qz*qw)).asin()), + rad((two * (qx*qw - qy*qz)).atan2(one - two*(sqx + sqz))), + ) + } + } } impl ToMatrix3 for Quaternion { @@ -384,15 +423,16 @@ impl Rotation3 for Quaternion { Quaternion::from_sv(c, axis.mul_s(s)) } + /// - [Maths - Conversion Euler to Quaternion] + /// (http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm) fn from_euler(x: Rad, y: Rad, z: Rad) -> Quaternion { - // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Conversion - let (sx2, cx2) = sin_cos(x.mul_s(cast(0.5f64).unwrap())); - let (sy2, cy2) = sin_cos(y.mul_s(cast(0.5f64).unwrap())); - let (sz2, cz2) = sin_cos(z.mul_s(cast(0.5f64).unwrap())); + let (s1, c1) = sin_cos(x.mul_s(cast(0.5f64).unwrap())); + let (s2, c2) = sin_cos(y.mul_s(cast(0.5f64).unwrap())); + let (s3, c3) = sin_cos(z.mul_s(cast(0.5f64).unwrap())); - Quaternion::new(cz2 * cx2 * cy2 + sz2 * sx2 * sy2, - sz2 * cx2 * cy2 - cz2 * sx2 * sy2, - cz2 * sx2 * cy2 + sz2 * cx2 * sy2, - cz2 * cx2 * sy2 - sz2 * sx2 * cy2) + Quaternion::new(c1 * c2 * c3 - s1 * s2 * s3, + s1 * s2 * c3 + c1 * c2 * s3, + s1 * c2 * c3 + c1 * s2 * s3, + c1 * s2 * c3 - s1 * c2 * s3) } } diff --git a/tests/quaternion.rs b/tests/quaternion.rs index 2aad708..3f64b68 100644 --- a/tests/quaternion.rs +++ b/tests/quaternion.rs @@ -20,6 +20,9 @@ extern crate cgmath; use cgmath::{ToMatrix4, ToMatrix3}; use cgmath::Quaternion; +use cgmath::{Rad, rad, ApproxEq}; +use cgmath::Rotation3; + #[test] fn to_matrix4() { @@ -30,3 +33,53 @@ fn to_matrix4() assert!(matrix_short == matrix_long); } + +#[test] +fn to_and_from_quaternion() +{ + fn eq(a: (Rad, Rad, Rad), b: (Rad, Rad, Rad)) { + let (ax, ay, az) = a; + let (bx, by, bz) = b; + if !(ax.approx_eq_eps(&bx, &0.001) && + ay.approx_eq_eps(&by, &0.001) && + az.approx_eq_eps(&bz, &0.001)) { + fail!("{} != {}", a, b) + } + } + + let hpi = Float::frac_pi_2(); + + let zero: Quaternion = Rotation3::from_euler(rad(0f32), rad(0f32), rad(0f32)); + eq((rad(0f32), rad(0f32), rad(0f32)), zero.to_euler()); + + let x_1: Quaternion = Rotation3::from_euler(rad(1f32), rad(0f32), rad(0f32)); + eq((rad(1f32), rad(0f32), rad(0f32)), x_1.to_euler()); + + let y_1: Quaternion = Rotation3::from_euler(rad(0f32), rad(1f32), rad(0f32)); + eq((rad(0f32), rad(1f32), rad(0f32)), y_1.to_euler()); + + let z_1: Quaternion = Rotation3::from_euler(rad(0f32), rad(0f32), rad(1f32)); + eq((rad(0f32), rad(0f32), rad(1f32)), z_1.to_euler()); + + let x_n1: Quaternion = Rotation3::from_euler(rad(-1f32), rad(0f32), rad(0f32)); + eq((rad(-1f32), rad(0f32), rad(0f32)), x_n1.to_euler()); + + let y_n1: Quaternion = Rotation3::from_euler(rad(0f32), rad(-1f32), rad(0f32)); + eq((rad(0f32), rad(-1f32), rad(0f32)), y_n1.to_euler()); + + let z_n1: Quaternion = Rotation3::from_euler(rad(0f32), rad(0f32), rad(-1f32)); + eq((rad(0f32), rad(0f32), rad(-1f32)), z_n1.to_euler()); + + let xzy_1: Quaternion = Rotation3::from_euler(rad(1f32), rad(1f32), rad(1f32)); + eq((rad(1f32), rad(1f32), rad(1f32)), xzy_1.to_euler()); + + let xzy_n1: Quaternion = Rotation3::from_euler(rad(-1f32), rad(-1f32), rad(-1f32)); + eq((rad(-1f32), rad(-1f32), rad(-1f32)), xzy_n1.to_euler()); + + let xzy_hp: Quaternion = Rotation3::from_euler(rad(0f32), rad(hpi), rad(1f32)); + eq((rad(0f32), rad(hpi), rad(1f32)), xzy_hp.to_euler()); + + let xzy_nhp: Quaternion = Rotation3::from_euler(rad(0f32), rad(-hpi), rad(1f32)); + eq((rad(0f32), rad(-hpi), rad(1f32)), xzy_nhp.to_euler()); + +}