1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
// 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;

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)]`.
///
/// 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
/// [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<Euler<A>>` 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 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:
///
/// ```
/// use cgmath::{Deg, Euler, Quaternion};
///
/// let rotation = Quaternion::from(Euler {
///     x: Deg(90.0),
///     y: Deg(45.0),
///     z: Deg(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)]
#[cfg_attr(feature = "rustc-serialize", derive(RustcEncodable, RustcDecodable))]
#[cfg_attr(feature = "eders", derive(Serialize, Deserialize))]
pub struct Euler<A: Angle> {
    /// The angle to apply around the _x_ axis. Also known at the _pitch_.
    pub x: A,
    /// The angle to apply around the _y_ axis. Also known at the _yaw_.
    pub y: A,
    /// The angle to apply around the _z_ axis. Also known at the _roll_.
    pub z: A,
}

impl<A: Angle> Euler<A> {
    /// Construct a set of euler angles.
    ///
    /// # Arguments
    ///
    /// * `x` - The angle to apply around the _x_ axis. Also known at the _pitch_.
    /// * `y` - The angle to apply around the _y_ axis. Also known at the _yaw_.
    /// * `z` - The angle to apply around the _z_ axis. Also known at the _roll_.
    pub fn new(x: A, y: A, z: A) -> Euler<A> {
        Euler { x: x, y: y, z: z }
    }
}

impl<S: BaseFloat> From<Quaternion<S>> for Euler<Rad<S>> {
    fn from(src: Quaternion<S>) -> Euler<Rad<S>> {
        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 + 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::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::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::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)),
            }
        }
    }
}

impl<A: Angle> ApproxEq for Euler<A> {
    type Epsilon = A::Unitless;

    #[inline]
    fn approx_eq_eps(&self, other: &Euler<A>, 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<A: Angle + Rand> Rand for Euler<A> {
    #[inline]
    fn rand<R: Rng>(rng: &mut R) -> Euler<A> {
        Euler { x: rng.gen(), y: rng.gen(), z: rng.gen() }
    }
}