From 467e87f3d33829b0f5fc26bb10d8e31b07df78d1 Mon Sep 17 00:00:00 2001 From: Jordan Miner Date: Sat, 30 Apr 2016 23:31:32 -0500 Subject: [PATCH] Fix Euler angles to quaternion conversion and vise versa Add tests that rotate a vector in all three axes, and tests to check the axis rotation sequence. --- CHANGELOG.md | 5 ++++ src/euler.rs | 49 +++++++++++++++++++++++++++------------ src/quaternion.rs | 11 +++++---- tests/quaternion.rs | 56 +++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 102 insertions(+), 19 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index c03650d..5ed8577 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,11 @@ This project adheres to [Semantic Versioning](http://semver.org/). ## [Unreleased] +### Changed + +- Fix quaternion to Euler angles and Euler angles to quaternion conversion. The axes are now + correct, and the order the angles are applied is XYZ. + ## [v0.9.1] - 2016-04-20 ### Changed diff --git a/src/euler.rs b/src/euler.rs index ec28b45..4a20794 100644 --- a/src/euler.rs +++ b/src/euler.rs @@ -27,6 +27,17 @@ use num::BaseFloat; /// /// This type is marked as `#[repr(C, packed)]`. /// +/// The axis rotation sequence is XYZ. That is, the rotation is first around +/// the X axis, then the Y axis, and lastly the Z axis (using intrinsic +/// rotations). Since all three rotation axes are used, the angles are +/// Tait–Bryan angles rather than proper Euler angles. +/// +/// # Ranges +/// +/// - x: [-pi, pi] +/// - y: [-pi/2, pi/2] +/// - z: [-pi, pi] +/// /// # Defining rotations using Euler angles /// /// Note that while [Euler angles] are intuitive to define, they are prone to @@ -42,9 +53,9 @@ use num::BaseFloat; /// /// For example, to define a quaternion that applies the following: /// -/// 1. a 45° rotation around the _x_ axis -/// 2. a 180° rotation around the _y_ axis -/// 3. a -30° rotation around the _z_ axis +/// 1. a 90° rotation around the _x_ axis +/// 2. a 45° rotation around the _y_ axis +/// 3. a 15° rotation around the _z_ axis /// /// you can use the following code: /// @@ -52,8 +63,8 @@ use num::BaseFloat; /// use cgmath::{Deg, Euler, Quaternion}; /// /// let rotation = Quaternion::from(Euler { -/// x: Deg::new(45.0), -/// y: Deg::new(180.0), +/// x: Deg::new(90.0), +/// y: Deg::new(45.0), /// z: Deg::new(15.0), /// }); /// ``` @@ -96,26 +107,34 @@ impl From> for Euler> { let (qw, qx, qy, qz) = (src.s, src.v.x, src.v.y, src.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; + let unit = sqx + sqz + sqy + sqw; + let test = qx * qz + qy * qw; + // We set x to zero and z to the value, but the other way would work too. if test > sig * unit { + // x + z = 2 * atan(x / w) Euler { - x: Rad::turn_div_4(), - y: Rad::zero(), + x: Rad::zero(), + y: Rad::turn_div_4(), z: Rad::atan2(qx, qw) * two, } } else if test < -sig * unit { + // x - z = 2 * atan(x / w) Euler { - x: -Rad::turn_div_4(), - y: Rad::zero(), - z: Rad::atan2(qx, qw) * two, + x: Rad::zero(), + y: -Rad::turn_div_4(), + z: -Rad::atan2(qx, qw) * two, } } else { + // Using the quat-to-matrix equation from either + // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm + // or equation 15 on page 7 of + // http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf + // to fill in the equations on page A-2 of the NASA document gives the below. Euler { - x: Rad::asin(two * (qx * qy + qz * qw)), - y: Rad::atan2(two * (qy * qw - qx * qz), one - two * (sqy + sqz)), - z: Rad::atan2(two * (qx * qw - qy * qz), one - two * (sqx + sqz)), + x: Rad::atan2(two * (-qy * qz + qx * qw), one - two * (sqx + sqy)), + y: Rad::asin(two * (qx * qz + qy * qw)), + z: Rad::atan2(two * (-qx * qy + qz * qw), one - two * (sqy + sqz)), } } } diff --git a/src/quaternion.rs b/src/quaternion.rs index 1b5a1ff..a235c10 100644 --- a/src/quaternion.rs +++ b/src/quaternion.rs @@ -156,17 +156,20 @@ impl From> for Quaternion<::Unitless> where A: Angle + Into::Unitless>>, { fn from(src: Euler) -> Quaternion { + // Euclidean Space has an Euler to quat equation, but it is for a different order (YXZ): // http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm + // Page A-2 here has the formula for XYZ: + // http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf let half = cast(0.5f64).unwrap(); let (s_x, c_x) = Rad::sin_cos(src.x.into() * half); let (s_y, c_y) = Rad::sin_cos(src.y.into() * half); let (s_z, c_z) = Rad::sin_cos(src.z.into() * half); - Quaternion::new(c_y * c_x * c_z - s_y * s_x * s_z, - s_y * s_x * c_z + c_y * c_x * s_z, - s_y * c_x * c_z + c_y * s_x * s_z, - c_y * s_x * c_z - s_y * c_x * s_z) + Quaternion::new(-s_x * s_y * s_z + c_x * c_y * c_z, + s_x * c_y * c_z + s_y * s_z * c_x, + -s_x * s_z * c_y + s_y * c_x * c_z, + s_x * s_y * c_z + s_z * c_x * c_y) } } diff --git a/tests/quaternion.rs b/tests/quaternion.rs index fd53821..10a47af 100644 --- a/tests/quaternion.rs +++ b/tests/quaternion.rs @@ -112,3 +112,59 @@ mod from { } } } + +mod rotate_from_euler { + use cgmath::*; + + #[test] + fn test_x() { + let vec = vec3(0.0, 0.0, 1.0); + + let rot = Quaternion::from(Euler::new(deg(90.0).into(), rad(0.0), rad(0.0))); + assert_approx_eq!(vec3(0.0, -1.0, 0.0), rot * vec); + + let rot = Quaternion::from(Euler::new(deg(-90.0).into(), rad(0.0), rad(0.0))); + assert_approx_eq!(vec3(0.0, 1.0, 0.0), rot * vec); + } + + #[test] + fn test_y() { + let vec = vec3(0.0, 0.0, 1.0); + + let rot = Quaternion::from(Euler::new(rad(0.0), deg(90.0).into(), rad(0.0))); + assert_approx_eq!(vec3(1.0, 0.0, 0.0), rot * vec); + + let rot = Quaternion::from(Euler::new(rad(0.0), deg(-90.0).into(), rad(0.0))); + assert_approx_eq!(vec3(-1.0, 0.0, 0.0), rot * vec); + } + + #[test] + fn test_z() { + let vec = vec3(1.0, 0.0, 0.0); + + let rot = Quaternion::from(Euler::new(rad(0.0), rad(0.0), deg(90.0).into())); + assert_approx_eq!(vec3(0.0, 1.0, 0.0), rot * vec); + + let rot = Quaternion::from(Euler::new(rad(0.0), rad(0.0), deg(-90.0).into())); + assert_approx_eq!(vec3(0.0, -1.0, 0.0), rot * vec); + } + + + // tests that the Y rotation is done after the X + #[test] + fn test_x_then_y() { + let vec = vec3(0.0, 1.0, 0.0); + + let rot = Quaternion::from(Euler::new(deg(90.0).into(), deg(90.0).into(), rad(0.0))); + assert_approx_eq!(vec3(0.0, 0.0, 1.0), rot * vec); + } + + // tests that the Z rotation is done after the Y + #[test] + fn test_y_then_z() { + let vec = vec3(0.0, 0.0, 1.0); + + let rot = Quaternion::from(Euler::new(rad(0.0), deg(90.0).into(), deg(90.0).into())); + assert_approx_eq!(vec3(1.0, 0.0, 0.0), rot * vec); + } +}