Merge pull request #359 from jminer/rotation-fixes

Rotation fixes

CHANGELOG.md

@@ -6,6 +6,13 @@ This project adheres to [Semantic Versioning](http://semver.org/).
 
 ## [Unreleased]
 
+### Changed
+
+- Fix Euler angles to quaternion and quaternion to Euler angles conversion. The axes are now
+  correct, and the order the angles are applied is XYZ. The conversion now matches the conversion
+  from axis angle.
+- Fix Euler angles to matrix conversion.
+
 ## [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
+/// 1. a 90° rotation around the _x_ axis
-/// 2. a 180° rotation around the _y_ axis
+/// 2. a 45° rotation around the _y_ axis
-/// 3. a -30° rotation around the _z_ 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),
+///     x: Deg::new(90.0),
-///     y: Deg::new(180.0),
+///     y: Deg::new(45.0),
 ///     z: Deg::new(15.0),
 /// });
 /// ```
@@ -97,26 +108,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 unit = sqx + sqz + sqy + sqw;
-        let test = qx * qy + qz * qw;
+        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(),
+                x: Rad::zero(),
-                y: 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(),
+                x: Rad::zero(),
-                y: Rad::zero(),
+                y: -Rad::turn_div_4(),
-                z: Rad::atan2(qx, qw) * two,
+                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)),
+                x: Rad::atan2(two * (-qy * qz + qx * qw), one - two * (sqx + sqy)),
-                y: Rad::atan2(two * (qy * qw - qx * qz), one - two * (sqy + sqz)),
+                y: Rad::asin(two * (qx * qz + qy * qw)),
-                z: Rad::atan2(two * (qx * qw - qy * qz), one - two * (sqx + sqz)),
+                z: Rad::atan2(two * (-qx * qy + qz * qw), one - two * (sqy + sqz)),
             }
         }
     }

src/matrix.rs

@@ -167,6 +167,8 @@ impl<S: BaseFloat> Matrix3<S> {
     }
 
     /// Create a rotation matrix from an angle around an arbitrary axis.
+    ///
+    /// The specified axis **must be normalized**, or it represents an invalid rotation.
     pub fn from_axis_angle(axis: Vector3<S>, angle: Rad<S>) -> Matrix3<S> {
         let (s, c) = Rad::sin_cos(angle);
         let _1subc = S::one() - c;
@@ -272,6 +274,8 @@ impl<S: BaseFloat> Matrix4<S> {
     }
 
     /// Create a homogeneous transformation matrix from an angle around an arbitrary axis.
+    ///
+    /// The specified axis **must be normalized**, or it represents an invalid rotation.
     pub fn from_axis_angle(axis: Vector3<S>, angle: Rad<S>) -> Matrix4<S> {
         let (s, c) = Rad::sin_cos(angle);
         let _1subc = S::one() - c;
@@ -1002,14 +1006,14 @@ impl<A> From<Euler<A>> for Matrix3<<A as Angle>::Unitless> where
     A: Angle + Into<Rad<<A as Angle>::Unitless>>,
 {
     fn from(src: Euler<A>) -> Matrix3<A::Unitless> {
-        // http://en.wikipedia.org/wiki/Rotation_matrix#General_rotations
+        // Page A-2: http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf
         let (sx, cx) = Rad::sin_cos(src.x.into());
         let (sy, cy) = Rad::sin_cos(src.y.into());
         let (sz, cz) = Rad::sin_cos(src.z.into());
 
-        Matrix3::new(cy * cz, cy * sz, -sy,
+        Matrix3::new(cy * cz, cx * sz + sx * sy * cz, sx * sz - cx * sy * cz,
-                     -cx * sz + sx * sy * cz, cx * cz + sx * sy * sz, sx * cy,
+                     -cy * sz, cx * cz - sx * sy * sz, sx * cz + cx * sy * sz,
-                     sx * sz + cx * sy * cz, -sx * cz + cx * sy * sz, cx * cy)
+                     sy, -sx * cy, cx * cy)
     }
 }
 
@@ -1017,14 +1021,14 @@ impl<A> From<Euler<A>> for Matrix4<<A as Angle>::Unitless> where
     A: Angle + Into<Rad<<A as Angle>::Unitless>>,
 {
     fn from(src: Euler<A>) -> Matrix4<A::Unitless> {
-        // http://en.wikipedia.org/wiki/Rotation_matrix#General_rotations
+        // Page A-2: http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf
         let (sx, cx) = Rad::sin_cos(src.x.into());
         let (sy, cy) = Rad::sin_cos(src.y.into());
         let (sz, cz) = Rad::sin_cos(src.z.into());
 
-        Matrix4::new(cy * cz, cy * sz, -sy, A::Unitless::zero(),
+        Matrix4::new(cy * cz, cx * sz + sx * sy * cz, sx * sz - cx * sy * cz, A::Unitless::zero(),
-                     -cx * sz + sx * sy * cz, cx * cz + sx * sy * sz, sx * cy, A::Unitless::zero(),
+                     -cy * sz, cx * cz - sx * sy * sz, sx * cz + cx * sy * sz, A::Unitless::zero(),
-                     sx * sz + cx * sy * cz, -sx * cz + cx * sy * sz, cx * cy, A::Unitless::zero(),
+                     sy, -sx * cy, cx * cy, A::Unitless::zero(),
                      A::Unitless::zero(), A::Unitless::zero(), A::Unitless::zero(), A::Unitless::one())
     }
 }

src/quaternion.rs

@@ -158,17 +158,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,
+        Quaternion::new(-s_x * s_y * s_z + c_x * c_y * c_z,
-                        s_y * s_x * c_z + c_y * c_x * s_z,
+                         s_x * c_y * c_z + s_y * s_z * c_x,
-                        s_y * c_x * c_z + c_y * s_x * s_z,
+                        -s_x * s_z * c_y + s_y * c_x * c_z,
-                        c_y * s_x * c_z - s_y * c_x * s_z)
+                         s_x * s_y * c_z + s_z * c_x * c_y)
     }
 }
 

src/rotation.rs

@@ -72,6 +72,8 @@ pub trait Rotation3<S: BaseFloat>: Rotation<Point3<S>>
                                  + Into<Quaternion<S>>
                                  + From<Euler<Rad<S>>> {
     /// Create a rotation using an angle around a given axis.
+    ///
+    /// The specified axis **must be normalized**, or it represents an invalid rotation.
     fn from_axis_angle(axis: Vector3<S>, angle: Rad<S>) -> Self;
 
     /// Create a rotation from an angle around the `x` axis (pitch).

tests/matrix.rs

@@ -398,6 +398,115 @@ pub mod matrix3 {
             #[test] fn test_neg_1()     { check_from_axis_angle_z(rad(-1.0)); }
         }
     }
+
+    mod rotate_from_euler {
+        use cgmath::*;
+
+        #[test]
+        fn test_x() {
+            let vec = vec3(0.0, 0.0, 1.0);
+
+            let rot = Matrix3::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 = Matrix3::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 = Matrix3::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 = Matrix3::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 = Matrix3::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 = Matrix3::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 = Matrix3::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 = Matrix3::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);
+        }
+    }
+
+    mod rotate_from_axis_angle {
+        use cgmath::*;
+
+        #[test]
+        fn test_x() {
+            let vec = vec3(0.0, 0.0, 1.0);
+
+            let rot = Matrix3::from_angle_x(deg(90.0).into());
+            println!("x mat: {:?}", rot);
+            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 = Matrix3::from_angle_y(deg(90.0).into());
+            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 = Matrix3::from_angle_z(deg(90.0).into());
+            assert_approx_eq!(vec3(0.0, 1.0, 0.0), rot * vec);
+        }
+
+        #[test]
+        fn test_xy() {
+            let vec = vec3(0.0, 0.0, 1.0);
+
+            let rot = Matrix3::from_axis_angle(vec3(1.0, 1.0, 0.0).normalize(), deg(90.0).into());
+            assert_approx_eq!(vec3(2.0f32.sqrt() / 2.0, -2.0f32.sqrt() / 2.0, 0.0), rot * vec);
+        }
+
+        #[test]
+        fn test_yz() {
+            let vec = vec3(1.0, 0.0, 0.0);
+
+            let rot = Matrix3::from_axis_angle(vec3(0.0, 1.0, 1.0).normalize(), deg(-90.0).into());
+            assert_approx_eq!(vec3(0.0, -2.0f32.sqrt() / 2.0, 2.0f32.sqrt() / 2.0), rot * vec);
+        }
+
+        #[test]
+        fn test_xz() {
+            let vec = vec3(0.0, 1.0, 0.0);
+
+            let rot = Matrix3::from_axis_angle(vec3(1.0, 0.0, 1.0).normalize(), deg(90.0).into());
+            assert_approx_eq!(vec3(-2.0f32.sqrt() / 2.0, 0.0, 2.0f32.sqrt() / 2.0), rot * vec);
+        }
+    }
 }
 
 pub mod matrix4 {

tests/quaternion.rs

@@ -112,3 +112,111 @@ 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);
+    }
+}
+
+mod rotate_from_axis_angle {
+    use cgmath::*;
+
+    #[test]
+    fn test_x() {
+        let vec = vec3(0.0, 0.0, 1.0);
+
+        let rot = Quaternion::from_angle_x(deg(90.0).into());
+        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_angle_y(deg(90.0).into());
+        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_angle_z(deg(90.0).into());
+        assert_approx_eq!(vec3(0.0, 1.0, 0.0), rot * vec);
+    }
+
+    #[test]
+    fn test_xy() {
+        let vec = vec3(0.0, 0.0, 1.0);
+
+        let rot = Quaternion::from_axis_angle(vec3(1.0, 1.0, 0.0).normalize(), deg(90.0).into());
+        assert_approx_eq!(vec3(2.0f32.sqrt() / 2.0, -2.0f32.sqrt() / 2.0, 0.0), rot * vec);
+    }
+
+    #[test]
+    fn test_yz() {
+        let vec = vec3(1.0, 0.0, 0.0);
+
+        let rot = Quaternion::from_axis_angle(vec3(0.0, 1.0, 1.0).normalize(), deg(-90.0).into());
+        assert_approx_eq!(vec3(0.0, -2.0f32.sqrt() / 2.0, 2.0f32.sqrt() / 2.0), rot * vec);
+    }
+
+    #[test]
+    fn test_xz() {
+        let vec = vec3(0.0, 1.0, 0.0);
+
+        let rot = Quaternion::from_axis_angle(vec3(1.0, 0.0, 1.0).normalize(), deg(90.0).into());
+        assert_approx_eq!(vec3(-2.0f32.sqrt() / 2.0, 0.0, 2.0f32.sqrt() / 2.0), rot * vec);
+    }
+}