Merge pull request #359 from jminer/rotation-fixes

Rotation fixes
This commit is contained in:
Brendan Zabarauskas 2016-05-18 00:19:43 +10:00
commit 7147180936
7 changed files with 279 additions and 27 deletions

View file

@ -6,6 +6,13 @@ This project adheres to [Semantic Versioning](http://semver.org/).
## [Unreleased] ## [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 ## [v0.9.1] - 2016-04-20
### Changed ### Changed

View file

@ -27,6 +27,17 @@ use num::BaseFloat;
/// ///
/// This type is marked as `#[repr(C, packed)]`. /// 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
/// TaitBryan angles rather than proper Euler angles.
///
/// # Ranges
///
/// - x: [-pi, pi]
/// - y: [-pi/2, pi/2]
/// - z: [-pi, pi]
///
/// # Defining rotations using Euler angles /// # Defining rotations using Euler angles
/// ///
/// Note that while [Euler angles] are intuitive to define, they are prone to /// 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: /// 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: /// you can use the following code:
/// ///
@ -52,8 +63,8 @@ use num::BaseFloat;
/// use cgmath::{Deg, Euler, Quaternion}; /// use cgmath::{Deg, Euler, Quaternion};
/// ///
/// let rotation = Quaternion::from(Euler { /// 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), /// 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 (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 (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 { if test > sig * unit {
// x + z = 2 * atan(x / w)
Euler { 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 if test < -sig * unit { } else if test < -sig * unit {
// x - z = 2 * atan(x / w)
Euler { 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 { } 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 { 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)),
} }
} }
} }

View file

@ -167,6 +167,8 @@ impl<S: BaseFloat> Matrix3<S> {
} }
/// Create a rotation matrix from an angle around an arbitrary axis. /// 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> { pub fn from_axis_angle(axis: Vector3<S>, angle: Rad<S>) -> Matrix3<S> {
let (s, c) = Rad::sin_cos(angle); let (s, c) = Rad::sin_cos(angle);
let _1subc = S::one() - c; 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. /// 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> { pub fn from_axis_angle(axis: Vector3<S>, angle: Rad<S>) -> Matrix4<S> {
let (s, c) = Rad::sin_cos(angle); let (s, c) = Rad::sin_cos(angle);
let _1subc = S::one() - c; 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>>, A: Angle + Into<Rad<<A as Angle>::Unitless>>,
{ {
fn from(src: Euler<A>) -> Matrix3<A::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 (sx, cx) = Rad::sin_cos(src.x.into());
let (sy, cy) = Rad::sin_cos(src.y.into()); let (sy, cy) = Rad::sin_cos(src.y.into());
let (sz, cz) = Rad::sin_cos(src.z.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>>, A: Angle + Into<Rad<<A as Angle>::Unitless>>,
{ {
fn from(src: Euler<A>) -> Matrix4<A::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 (sx, cx) = Rad::sin_cos(src.x.into());
let (sy, cy) = Rad::sin_cos(src.y.into()); let (sy, cy) = Rad::sin_cos(src.y.into());
let (sz, cz) = Rad::sin_cos(src.z.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()) A::Unitless::zero(), A::Unitless::zero(), A::Unitless::zero(), A::Unitless::one())
} }
} }

View file

@ -158,17 +158,20 @@ impl<A> From<Euler<A>> for Quaternion<<A as Angle>::Unitless> where
A: Angle + Into<Rad<<A as Angle>::Unitless>>, A: Angle + Into<Rad<<A as Angle>::Unitless>>,
{ {
fn from(src: Euler<A>) -> Quaternion<A::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 // 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 half = cast(0.5f64).unwrap();
let (s_x, c_x) = Rad::sin_cos(src.x.into() * half); 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_y, c_y) = Rad::sin_cos(src.y.into() * half);
let (s_z, c_z) = Rad::sin_cos(src.z.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)
} }
} }

View file

@ -72,6 +72,8 @@ pub trait Rotation3<S: BaseFloat>: Rotation<Point3<S>>
+ Into<Quaternion<S>> + Into<Quaternion<S>>
+ From<Euler<Rad<S>>> { + From<Euler<Rad<S>>> {
/// Create a rotation using an angle around a given axis. /// 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; fn from_axis_angle(axis: Vector3<S>, angle: Rad<S>) -> Self;
/// Create a rotation from an angle around the `x` axis (pitch). /// Create a rotation from an angle around the `x` axis (pitch).

View file

@ -398,6 +398,115 @@ pub mod matrix3 {
#[test] fn test_neg_1() { check_from_axis_angle_z(rad(-1.0)); } #[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 { pub mod matrix4 {

View file

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