diff --git a/benches/construction.rs b/benches/construction.rs index c49c08d..f80d504 100644 --- a/benches/construction.rs +++ b/benches/construction.rs @@ -21,7 +21,7 @@ extern crate cgmath; use rand::{IsaacRng, Rng}; use test::Bencher; -use cgmath::{Quaternion, Basis2, Basis3, Vector3, Rotation2, Rotation3, Rad}; +use cgmath::*; #[path="common/macros.rs"] #[macro_use] mod macros; @@ -55,7 +55,7 @@ fn _bench_rot3_from_axisangle(bh: &mut Bencher) { bench_from_axis_angle::>(bh) } -bench_construction!(_bench_rot2_from_axisangle, Basis2, Rotation2::from_angle [ angle: Rad ]); +bench_construction!(_bench_rot2_from_axisangle, Basis2, Basis2::from_angle [ angle: Rad ]); -bench_construction!(_bench_quat_from_euler_angles, Quaternion, Rotation3::from_euler [roll: Rad, pitch: Rad, yaw: Rad]); -bench_construction!(_bench_rot3_from_euler_angles, Basis3, Rotation3::from_euler [roll: Rad, pitch: Rad, yaw: Rad]); +bench_construction!(_bench_quat_from_euler_angles, Quaternion, Quaternion::from [src: Euler>]); +bench_construction!(_bench_rot3_from_euler_angles, Basis3, Basis3::from [src: Euler>]); diff --git a/src/euler.rs b/src/euler.rs new file mode 100644 index 0000000..bd5f618 --- /dev/null +++ b/src/euler.rs @@ -0,0 +1,127 @@ +// Copyright 2016 The CGMath Developers. For a full listing of the authors, +// refer to the Cargo.toml file at the top-level directory of this distribution. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use rand::{Rand, Rng}; +use num_traits::{cast, Zero}; + +use structure::*; + +use angle::Rad; +use approx::ApproxEq; +use quaternion::Quaternion; +use num::BaseFloat; + +/// A set of [Euler angles] representing a rotation in three-dimensional space. +/// +/// This type is marked as `#[repr(C, packed)]`. +/// +/// # Defining rotations using Euler angles +/// +/// Note that while [Euler angles] are intuitive to define, they are prone to +/// [gimbal lock] and are challenging to interpolate between. Instead we +/// recommend that you convert them to a more robust representation, such as a +/// quaternion or or rotation matrix. To this end, `From>` conversions +/// are provided for the following types: +/// +/// - [`Basis3`](struct.Basis3.html) +/// - [`Matrix3`](struct.Matrix3.html) +/// - [`Matrix4`](struct.Matrix4.html) +/// - [`Quaternion`](struct.Quaternion.html) +/// +/// 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 +/// +/// you can use the following code: +/// +/// ``` +/// use cgmath::{Deg, Euler, Quaternion}; +/// +/// let rotation = Quaternion::from(Euler { +/// x: Deg::new(45.0), +/// y: Deg::new(180.0), +/// z: Deg::new(15.0), +/// }); +/// ``` +/// +/// [Euler angles]: https://en.wikipedia.org/wiki/Euler_angles +/// [gimbal lock]: https://en.wikipedia.org/wiki/Gimbal_lock#Gimbal_lock_in_applied_mathematics +/// [convert]: #defining-rotations-using-euler-angles +#[repr(C, packed)] +#[derive(Copy, Clone, Debug)] +#[derive(PartialEq, Eq)] +#[derive(RustcEncodable, RustcDecodable)] +pub struct Euler { + /// The angle to apply around the _x_ axis. + pub x: A, + /// The angle to apply around the _y_ axis. + pub y: A, + /// The angle to apply around the _z_ axis. + pub z: A, +} + +impl From> for Euler> { + fn from(src: Quaternion) -> Euler> { + let sig: S = cast(0.499).unwrap(); + let two: S = cast(2).unwrap(); + let one: S = cast(1).unwrap(); + + 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; + + if test > sig * unit { + Euler { + x: Rad::turn_div_4(), + y: Rad::zero(), + z: Rad::atan2(qx, qw) * two, + } + } else if test < -sig * unit { + Euler { + x: -Rad::turn_div_4(), + y: Rad::zero(), + z: Rad::atan2(qx, qw) * two, + } + } else { + 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)), + } + } + } +} + +impl ApproxEq for Euler { + type Epsilon = A::Unitless; + + #[inline] + fn approx_eq_eps(&self, other: &Euler, epsilon: &A::Unitless) -> bool { + self.x.approx_eq_eps(&other.x, epsilon) && + self.y.approx_eq_eps(&other.y, epsilon) && + self.z.approx_eq_eps(&other.z, epsilon) + } +} + +impl Rand for Euler { + #[inline] + fn rand(rng: &mut R) -> Euler { + Euler { x: rng.gen(), y: rng.gen(), z: rng.gen() } + } +} diff --git a/src/lib.rs b/src/lib.rs index 2ed6501..4a01e8e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -64,6 +64,7 @@ pub use quaternion::Quaternion; pub use vector::{Vector2, Vector3, Vector4, dot, vec2, vec3, vec4}; pub use angle::{Deg, Rad, deg, rad}; +pub use euler::Euler; pub use point::{Point2, Point3}; pub use rotation::*; pub use transform::*; @@ -88,6 +89,7 @@ mod quaternion; mod vector; mod angle; +mod euler; mod point; mod rotation; mod transform; diff --git a/src/matrix.rs b/src/matrix.rs index 9110fb2..97c3089 100644 --- a/src/matrix.rs +++ b/src/matrix.rs @@ -25,6 +25,7 @@ use structure::*; use angle::Rad; use approx::ApproxEq; +use euler::Euler; use num::BaseFloat; use point::Point3; use quaternion::Quaternion; @@ -159,24 +160,6 @@ impl Matrix3 { S::zero(), S::zero(), S::one()) } - /// Create a rotation matrix from a set of euler angles. - /// - /// # Parameters - /// - /// - `x`: the angular rotation around the `x` axis (pitch). - /// - `y`: the angular rotation around the `y` axis (yaw). - /// - `z`: the angular rotation around the `z` axis (roll). - pub fn from_euler(x: Rad, y: Rad, z: Rad) -> Matrix3 { - // http://en.wikipedia.org/wiki/Rotation_matrix#General_rotations - let (sx, cx) = Rad::sin_cos(x); - let (sy, cy) = Rad::sin_cos(y); - let (sz, cz) = Rad::sin_cos(z); - - 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) - } - /// Create a rotation matrix from an angle around an arbitrary axis. pub fn from_axis_angle(axis: Vector3, angle: Rad) -> Matrix3 { let (s, c) = Rad::sin_cos(angle); @@ -196,6 +179,37 @@ impl Matrix3 { } } +impl From> for Matrix3<::Unitless> where + A: Angle + Into::Unitless>>, +{ + fn from(src: Euler) -> Matrix3 { + // http://en.wikipedia.org/wiki/Rotation_matrix#General_rotations + 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) + } +} + +impl From> for Matrix4<::Unitless> where + A: Angle + Into::Unitless>>, +{ + fn from(src: Euler) -> Matrix4 { + // http://en.wikipedia.org/wiki/Rotation_matrix#General_rotations + 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(), + A::Unitless::zero(), A::Unitless::zero(), A::Unitless::zero(), A::Unitless::one()) + } +} + impl Matrix4 { /// Create a new matrix, providing values for each index. #[inline] diff --git a/src/quaternion.rs b/src/quaternion.rs index f1dbce2..1bc1054 100644 --- a/src/quaternion.rs +++ b/src/quaternion.rs @@ -24,6 +24,7 @@ use structure::*; use angle::Rad; use approx::ApproxEq; +use euler::Euler; use matrix::{Matrix3, Matrix4}; use num::BaseFloat; use point::Point3; @@ -117,44 +118,6 @@ impl Quaternion { (self * scale1 + other * scale2) * Rad::sin(theta).recip() } } - - /// Convert a Quaternion to Eular angles - /// This is a polar singularity aware conversion - /// - /// Based on: - /// - [Maths - Conversion Quaternion to Euler] - /// (http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/) - pub fn to_euler(self) -> (Rad, Rad, Rad) { - let sig: S = cast(0.499f64).unwrap(); - let two: S = cast(2f64).unwrap(); - let one: S = cast(1f64).unwrap(); - - let (qw, qx, qy, qz) = (self.s, self.v.x, self.v.y, self.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; - - if test > sig * unit { - ( - Rad::zero(), - Rad::turn_div_4(), - Rad::atan2(qx, qw) * two, - ) - } else if test < -sig * unit { - ( - Rad::zero(), - -Rad::turn_div_4(), - Rad::atan2(qx, qw) * two, - ) - } else { - ( - Rad::atan2(two * (qy * qw - qx * qz), one - two * (sqy + sqz)), - Rad::asin(two * (qx * qy + qz * qw)), - Rad::atan2(two * (qx * qw - qy * qz), one - two * (sqx + sqz)), - ) - } - } } impl VectorSpace for Quaternion { @@ -173,6 +136,24 @@ impl InnerSpace for Quaternion { } } +impl From> for Quaternion<::Unitless> where + A: Angle + Into::Unitless>>, +{ + fn from(src: Euler) -> Quaternion { + // http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm + + 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) + } +} + impl_operator!( Neg for Quaternion { fn neg(quat) -> Quaternion { Quaternion::from_sv(-quat.s, -quat.v) @@ -373,19 +354,6 @@ impl Rotation3 for Quaternion { let (s, c) = Rad::sin_cos(angle * cast(0.5f64).unwrap()); Quaternion::from_sv(c, axis * s) } - - /// - [Maths - Conversion Euler to Quaternion] - /// (http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm) - fn from_euler(x: Rad, y: Rad, z: Rad) -> Quaternion { - let (s1, c1) = Rad::sin_cos(x * cast(0.5f64).unwrap()); - let (s2, c2) = Rad::sin_cos(y * cast(0.5f64).unwrap()); - let (s3, c3) = Rad::sin_cos(z * cast(0.5f64).unwrap()); - - Quaternion::new(c1 * c2 * c3 - s1 * s2 * s3, - s1 * s2 * c3 + c1 * c2 * s3, - s1 * c2 * c3 + c1 * s2 * s3, - c1 * s2 * c3 - s1 * c2 * s3) - } } impl Into<[S; 4]> for Quaternion { diff --git a/src/rotation.rs b/src/rotation.rs index ce362d4..340fb0e 100644 --- a/src/rotation.rs +++ b/src/rotation.rs @@ -19,6 +19,7 @@ use structure::*; use angle::Rad; use approx::ApproxEq; +use euler::Euler; use matrix::{Matrix2, Matrix3}; use num::BaseFloat; use point::{Point2, Point3}; @@ -85,19 +86,11 @@ pub trait Rotation2: Rotation> pub trait Rotation3: Rotation> + Into> + Into> - + Into> { + + Into> + + From>> { /// Create a rotation using an angle around a given axis. fn from_axis_angle(axis: Vector3, angle: Rad) -> Self; - /// Create a rotation from a set of euler angles. - /// - /// # Parameters - /// - /// - `x`: the angular rotation around the `x` axis (pitch). - /// - `y`: the angular rotation around the `y` axis (yaw). - /// - `z`: the angular rotation around the `z` axis (roll). - fn from_euler(x: Rad, y: Rad, z: Rad) -> Self; - /// Create a rotation from an angle around the `x` axis (pitch). #[inline] fn from_angle_x(theta: Rad) -> Self { @@ -317,10 +310,6 @@ impl Rotation3 for Basis3 { Basis3 { mat: Matrix3::from_axis_angle(axis, angle) } } - fn from_euler(x: Rad, y: Rad, z: Rad) -> Basis3 { - Basis3 { mat: Matrix3::from_euler(x, y ,z) } - } - fn from_angle_x(theta: Rad) -> Basis3 { Basis3 { mat: Matrix3::from_angle_x(theta) } } @@ -334,6 +323,17 @@ impl Rotation3 for Basis3 { } } +impl From> for Basis3<::Unitless> where + A: Into::Unitless>>, +{ + /// Create a three-dimensional rotation matrix from a set of euler angles. + fn from(src: Euler) -> Basis3 { + Basis3 { + mat: Matrix3::from(src), + } + } +} + impl fmt::Debug for Basis3 { fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { try!(write!(f, "Basis3 ")); diff --git a/tests/matrix.rs b/tests/matrix.rs index 08dfaca..10e2c83 100644 --- a/tests/matrix.rs +++ b/tests/matrix.rs @@ -332,7 +332,7 @@ pub mod matrix3 { fn check_from_axis_angle_x(pitch: Rad) { let found = Matrix3::from_angle_x(pitch); - let expected = Matrix3::from_euler(pitch, rad(0.0), rad(0.0)); + let expected = Matrix3::from(Euler { x: pitch, y: rad(0.0), z: rad(0.0) }); assert_approx_eq_eps!(found, expected, 0.001); } @@ -346,7 +346,7 @@ pub mod matrix3 { fn check_from_axis_angle_y(yaw: Rad) { let found = Matrix3::from_angle_y(yaw); - let expected = Matrix3::from_euler(rad(0.0), yaw, rad(0.0)); + let expected = Matrix3::from(Euler { x: rad(0.0), y: yaw, z: rad(0.0) }); assert_approx_eq_eps!(found, expected, 0.001); } @@ -360,7 +360,7 @@ pub mod matrix3 { fn check_from_axis_angle_z(roll: Rad) { let found = Matrix3::from_angle_z(roll); - let expected = Matrix3::from_euler(rad(0.0), rad(0.0), roll); + let expected = Matrix3::from(Euler { x: rad(0.0), y: rad(0.0), z: roll }); assert_approx_eq_eps!(found, expected, 0.001); } @@ -375,7 +375,7 @@ pub mod matrix3 { fn check_from_axis_angle_x(pitch: Rad) { let found = Matrix3::from_axis_angle(Vector3::unit_x(), pitch); - let expected = Matrix3::from_euler(pitch, rad(0.0), rad(0.0)); + let expected = Matrix3::from(Euler { x: pitch, y: rad(0.0), z: rad(0.0) }); assert_approx_eq_eps!(found, expected, 0.001); } @@ -389,7 +389,7 @@ pub mod matrix3 { fn check_from_axis_angle_y(yaw: Rad) { let found = Matrix3::from_axis_angle(Vector3::unit_y(), yaw); - let expected = Matrix3::from_euler(rad(0.0), yaw, rad(0.0)); + let expected = Matrix3::from(Euler { x: rad(0.0), y: yaw, z: rad(0.0) }); assert_approx_eq_eps!(found, expected, 0.001); } @@ -403,7 +403,7 @@ pub mod matrix3 { fn check_from_axis_angle_z(roll: Rad) { let found = Matrix3::from_axis_angle(Vector3::unit_z(), roll); - let expected = Matrix3::from_euler(rad(0.0), rad(0.0), roll); + let expected = Matrix3::from(Euler { x: rad(0.0), y: rad(0.0), z: roll }); assert_approx_eq_eps!(found, expected, 0.001); } diff --git a/tests/quaternion.rs b/tests/quaternion.rs index effa31e..fd53821 100644 --- a/tests/quaternion.rs +++ b/tests/quaternion.rs @@ -43,12 +43,12 @@ mod operators { #[test] fn test_mul() { - impl_test_mul!(2.0f32, Quaternion::from_euler(rad(1f32), rad(1f32), rad(1f32))); + impl_test_mul!(2.0f32, Quaternion::from(Euler { x: rad(1f32), y: rad(1f32), z: rad(1f32) })); } #[test] fn test_div() { - impl_test_div!(2.0f32, Quaternion::from_euler(rad(1f32), rad(1f32), rad(1f32))); + impl_test_div!(2.0f32, Quaternion::from(Euler { x: rad(1f32), y: rad(1f32), z: rad(1f32) })); } } @@ -57,28 +57,23 @@ mod to_from_euler { use cgmath::*; - fn check_euler(pitch: Rad, yaw: Rad, roll: Rad) { - let quat = Quaternion::from_euler(pitch, yaw, roll); - let (found_pitch, found_yaw, found_roll) = quat.to_euler(); - - assert_approx_eq_eps!(pitch, found_pitch, 0.001); - assert_approx_eq_eps!(yaw, found_yaw, 0.001); - assert_approx_eq_eps!(roll, found_roll, 0.001); + fn check_euler(rotation: Euler>) { + assert_approx_eq_eps!(Euler::from(Quaternion::from(rotation)), rotation, 0.001); } const HPI: f32 = f32::consts::FRAC_PI_2; - #[test] fn test_zero() { check_euler(rad(0f32), rad(0f32), rad(0f32)); } - #[test] fn test_yaw_pos_1() { check_euler(rad(0f32), rad(1f32), rad(0f32)); } - #[test] fn test_yaw_neg_1() { check_euler(rad(0f32), rad(-1f32), rad(0f32)); } - #[test] fn test_pitch_pos_1() { check_euler(rad(1f32), rad(0f32), rad(0f32)); } - #[test] fn test_pitch_neg_1() { check_euler(rad(-1f32), rad(0f32), rad(0f32)); } - #[test] fn test_roll_pos_1() { check_euler(rad(0f32), rad(0f32), rad(1f32)); } - #[test] fn test_roll_neg_1() { check_euler(rad(0f32), rad(0f32), rad(-1f32)); } - #[test] fn test_pitch_yaw_roll_pos_1() { check_euler(rad(1f32), rad(1f32), rad(1f32)); } - #[test] fn test_pitch_yaw_roll_neg_1() { check_euler(rad(-1f32), rad(-1f32), rad(-1f32)); } - #[test] fn test_pitch_yaw_roll_pos_hp() { check_euler(rad(0f32), rad(HPI), rad(1f32)); } - #[test] fn test_pitch_yaw_roll_neg_hp() { check_euler(rad(0f32), rad(-HPI), rad(1f32)); } + #[test] fn test_zero() { check_euler(Euler { x: rad( 0f32), y: rad( 0f32), z: rad( 0f32) }); } + #[test] fn test_yaw_pos_1() { check_euler(Euler { x: rad( 0f32), y: rad( 1f32), z: rad( 0f32) }); } + #[test] fn test_yaw_neg_1() { check_euler(Euler { x: rad( 0f32), y: rad(-1f32), z: rad( 0f32) }); } + #[test] fn test_pitch_pos_1() { check_euler(Euler { x: rad( 1f32), y: rad( 0f32), z: rad( 0f32) }); } + #[test] fn test_pitch_neg_1() { check_euler(Euler { x: rad(-1f32), y: rad( 0f32), z: rad( 0f32) }); } + #[test] fn test_roll_pos_1() { check_euler(Euler { x: rad( 0f32), y: rad( 0f32), z: rad( 1f32) }); } + #[test] fn test_roll_neg_1() { check_euler(Euler { x: rad( 0f32), y: rad( 0f32), z: rad(-1f32) }); } + #[test] fn test_pitch_yaw_roll_pos_1() { check_euler(Euler { x: rad( 1f32), y: rad( 1f32), z: rad( 1f32) }); } + #[test] fn test_pitch_yaw_roll_neg_1() { check_euler(Euler { x: rad(-1f32), y: rad(-1f32), z: rad(-1f32) }); } + #[test] fn test_pitch_yaw_roll_pos_hp() { check_euler(Euler { x: rad( 0f32), y: rad( HPI), z: rad( 1f32) }); } + #[test] fn test_pitch_yaw_roll_neg_hp() { check_euler(Euler { x: rad( 0f32), y: rad( -HPI), z: rad( 1f32) }); } } mod from { @@ -86,7 +81,7 @@ mod from { use cgmath::*; fn check_with_euler(x: Rad, y: Rad, z: Rad) { - let matrix3 = Matrix3::from_euler(x, y, z); + let matrix3 = Matrix3::from(Euler { x: x, y: y, z: z }); let quaternion = Quaternion::from(matrix3); let quaternion_matrix3 = Matrix3::from(quaternion); assert_approx_eq!(matrix3, quaternion_matrix3);