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

@@ -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

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<S: BaseFloat> From<Quaternion<S>> for Euler<Rad<S>> {
         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)),
             }
         }
     }

src/quaternion.rs

@@ -156,17 +156,20 @@ impl<A> From<Euler<A>> for Quaternion<<A as Angle>::Unitless> where
     A: Angle + Into<Rad<<A as Angle>::Unitless>>,
 {
     fn from(src: Euler<A>) -> Quaternion<A::Unitless> {
+        // 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)
     }
 }
 

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);
+    }
+}