From c11371794f4811753886818fb4a27ff9d49c795c Mon Sep 17 00:00:00 2001 From: Jordan Miner Date: Sun, 1 May 2016 03:02:18 -0500 Subject: [PATCH] Fix Euler angle to matrix conversion The equations were written with rows horizontally instead of vertically and some signs were wrong. --- CHANGELOG.md | 6 ++- src/matrix.rs | 16 +++---- tests/matrix.rs | 109 ++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 121 insertions(+), 10 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5ed8577..810f02d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,8 +8,10 @@ This project adheres to [Semantic Versioning](http://semver.org/). ### 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. +- 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 diff --git a/src/matrix.rs b/src/matrix.rs index 3f28998..1d2b5f1 100644 --- a/src/matrix.rs +++ b/src/matrix.rs @@ -1000,14 +1000,14 @@ impl From> for Matrix3<::Unitless> where A: Angle + Into::Unitless>>, { fn from(src: Euler) -> Matrix3 { - // 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, - -cx * sz + sx * sy * cz, cx * cz + sx * sy * sz, sx * cy, - sx * sz + cx * sy * cz, -sx * cz + cx * sy * sz, cx * cy) + Matrix3::new(cy * cz, cx * sz + sx * sy * cz, sx * sz - cx * sy * cz, + -cy * sz, cx * cz - sx * sy * sz, sx * cz + cx * sy * sz, + sy, -sx * cy, cx * cy) } } @@ -1015,14 +1015,14 @@ impl From> for Matrix4<::Unitless> where A: Angle + Into::Unitless>>, { fn from(src: Euler) -> Matrix4 { - // 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(), - -cx * sz + sx * sy * cz, cx * cz + sx * sy * sz, sx * cy, A::Unitless::zero(), - sx * sz + cx * sy * cz, -sx * cz + cx * sy * sz, cx * cy, A::Unitless::zero(), + Matrix4::new(cy * cz, cx * sz + sx * sy * cz, sx * sz - cx * sy * cz, A::Unitless::zero(), + -cy * sz, cx * cz - sx * sy * sz, sx * cz + cx * sy * sz, A::Unitless::zero(), + sy, -sx * cy, cx * cy, A::Unitless::zero(), A::Unitless::zero(), A::Unitless::zero(), A::Unitless::zero(), A::Unitless::one()) } } diff --git a/tests/matrix.rs b/tests/matrix.rs index d5c6f06..3af48f0 100644 --- a/tests/matrix.rs +++ b/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 {