glam/
euler.rs

Help
1/*
2Conversion from quaternions to Euler rotation sequences.
3
4From: http://bediyap.com/programming/convert-quaternion-to-euler-rotations/
5*/
6
7use crate::{DQuat, Quat};
8
9/// Euler rotation sequences.
10///
11/// The angles are applied starting from the right.
12/// E.g. XYZ will first apply the z-axis rotation.
13///
14/// YXZ can be used for yaw (y-axis), pitch (x-axis), roll (z-axis).
15#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
16pub enum EulerRot {
17    /// Intrinsic three-axis rotation ZYX
18    ZYX,
19    /// Intrinsic three-axis rotation ZXY
20    ZXY,
21    /// Intrinsic three-axis rotation YXZ
22    YXZ,
23    /// Intrinsic three-axis rotation YZX
24    YZX,
25    /// Intrinsic three-axis rotation XYZ
26    XYZ,
27    /// Intrinsic three-axis rotation XZY
28    XZY,
29}
30
31impl Default for EulerRot {
32    /// Default `YXZ` as yaw (y-axis), pitch (x-axis), roll (z-axis).
33    fn default() -> Self {
34        Self::YXZ
35    }
36}
37
38/// Conversion from quaternion to euler angles.
39pub(crate) trait EulerFromQuaternion<Q: Copy>: Sized + Copy {
40    type Output;
41    /// Compute the angle of the first axis (X-x-x)
42    fn first(self, q: Q) -> Self::Output;
43    /// Compute then angle of the second axis (x-X-x)
44    fn second(self, q: Q) -> Self::Output;
45    /// Compute then angle of the third axis (x-x-X)
46    fn third(self, q: Q) -> Self::Output;
47
48    /// Compute all angles of a rotation in the notation order
49    fn convert_quat(self, q: Q) -> (Self::Output, Self::Output, Self::Output);
50
51    #[doc(hidden)]
52    fn sine_theta(self, q: Q) -> Self::Output;
53}
54
55/// Conversion from euler angles to quaternion.
56pub(crate) trait EulerToQuaternion<T>: Copy {
57    type Output;
58    /// Create the rotation quaternion for the three angles of this euler rotation sequence.
59    fn new_quat(self, u: T, v: T, w: T) -> Self::Output;
60}
61
62macro_rules! impl_from_quat {
63    ($t:ident, $quat:ident) => {
64        impl EulerFromQuaternion<$quat> for EulerRot {
65            type Output = $t;
66
67            fn sine_theta(self, q: $quat) -> $t {
68                use EulerRot::*;
69                match self {
70                    ZYX => -2.0 * (q.x * q.z - q.w * q.y),
71                    ZXY => 2.0 * (q.y * q.z + q.w * q.x),
72                    YXZ => -2.0 * (q.y * q.z - q.w * q.x),
73                    YZX => 2.0 * (q.x * q.y + q.w * q.z),
74                    XYZ => 2.0 * (q.x * q.z + q.w * q.y),
75                    XZY => -2.0 * (q.x * q.y - q.w * q.z),
76                }
77                .clamp(-1.0, 1.0)
78            }
79
80            fn first(self, q: $quat) -> $t {
81                use crate::$t::math;
82                use EulerRot::*;
83
84                let sine_theta = self.sine_theta(q);
85                if math::abs(sine_theta) > 0.99999 {
86                    let scale = 2.0 * math::signum(sine_theta);
87
88                    match self {
89                        ZYX => scale * math::atan2(-q.x, q.w),
90                        ZXY => scale * math::atan2(q.y, q.w),
91                        YXZ => scale * math::atan2(-q.z, q.w),
92                        YZX => scale * math::atan2(q.x, q.w),
93                        XYZ => scale * math::atan2(q.z, q.w),
94                        XZY => scale * math::atan2(-q.y, q.w),
95                    }
96                } else {
97                    match self {
98                        ZYX => math::atan2(
99                            2.0 * (q.x * q.y + q.w * q.z),
100                            q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
101                        ),
102                        ZXY => math::atan2(
103                            -2.0 * (q.x * q.y - q.w * q.z),
104                            q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
105                        ),
106                        YXZ => math::atan2(
107                            2.0 * (q.x * q.z + q.w * q.y),
108                            q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
109                        ),
110                        YZX => math::atan2(
111                            -2.0 * (q.x * q.z - q.w * q.y),
112                            q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
113                        ),
114                        XYZ => math::atan2(
115                            -2.0 * (q.y * q.z - q.w * q.x),
116                            q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
117                        ),
118                        XZY => math::atan2(
119                            2.0 * (q.y * q.z + q.w * q.x),
120                            q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
121                        ),
122                    }
123                }
124            }
125
126            fn second(self, q: $quat) -> $t {
127                use crate::$t::math;
128                math::asin(self.sine_theta(q))
129            }
130
131            fn third(self, q: $quat) -> $t {
132                use crate::$t::math;
133                use EulerRot::*;
134                if math::abs(self.sine_theta(q)) > 0.99999 {
135                    0.0
136                } else {
137                    match self {
138                        ZYX => math::atan2(
139                            2.0 * (q.y * q.z + q.w * q.x),
140                            q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
141                        ),
142                        ZXY => math::atan2(
143                            -2.0 * (q.x * q.z - q.w * q.y),
144                            q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
145                        ),
146                        YXZ => math::atan2(
147                            2.0 * (q.x * q.y + q.w * q.z),
148                            q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
149                        ),
150                        YZX => math::atan2(
151                            -2.0 * (q.y * q.z - q.w * q.x),
152                            q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
153                        ),
154                        XYZ => math::atan2(
155                            -2.0 * (q.x * q.y - q.w * q.z),
156                            q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
157                        ),
158                        XZY => math::atan2(
159                            2.0 * (q.x * q.z + q.w * q.y),
160                            q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
161                        ),
162                    }
163                }
164            }
165
166            fn convert_quat(self, q: $quat) -> ($t, $t, $t) {
167                use crate::$t::math;
168                use EulerRot::*;
169
170                let sine_theta = self.sine_theta(q);
171                let second = math::asin(sine_theta);
172
173                if math::abs(sine_theta) > 0.99999 {
174                    let scale = 2.0 * math::signum(sine_theta);
175
176                    return match self {
177                        ZYX => (scale * math::atan2(-q.x, q.w), second, 0.0),
178                        ZXY => (scale * math::atan2(q.y, q.w), second, 0.0),
179                        YXZ => (scale * math::atan2(-q.z, q.w), second, 0.0),
180                        YZX => (scale * math::atan2(q.x, q.w), second, 0.0),
181                        XYZ => (scale * math::atan2(q.z, q.w), second, 0.0),
182                        XZY => (scale * math::atan2(-q.y, q.w), second, 0.0),
183                    };
184                }
185
186                let first = match self {
187                    ZYX => math::atan2(
188                        2.0 * (q.x * q.y + q.w * q.z),
189                        q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
190                    ),
191                    ZXY => math::atan2(
192                        -2.0 * (q.x * q.y - q.w * q.z),
193                        q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
194                    ),
195                    YXZ => math::atan2(
196                        2.0 * (q.x * q.z + q.w * q.y),
197                        q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
198                    ),
199                    YZX => math::atan2(
200                        -2.0 * (q.x * q.z - q.w * q.y),
201                        q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
202                    ),
203                    XYZ => math::atan2(
204                        -2.0 * (q.y * q.z - q.w * q.x),
205                        q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
206                    ),
207                    XZY => math::atan2(
208                        2.0 * (q.y * q.z + q.w * q.x),
209                        q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
210                    ),
211                };
212
213                let third = match self {
214                    ZYX => math::atan2(
215                        2.0 * (q.y * q.z + q.w * q.x),
216                        q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
217                    ),
218                    ZXY => math::atan2(
219                        -2.0 * (q.x * q.z - q.w * q.y),
220                        q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
221                    ),
222                    YXZ => math::atan2(
223                        2.0 * (q.x * q.y + q.w * q.z),
224                        q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
225                    ),
226                    YZX => math::atan2(
227                        -2.0 * (q.y * q.z - q.w * q.x),
228                        q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
229                    ),
230                    XYZ => math::atan2(
231                        -2.0 * (q.x * q.y - q.w * q.z),
232                        q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
233                    ),
234                    XZY => math::atan2(
235                        2.0 * (q.x * q.z + q.w * q.y),
236                        q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
237                    ),
238                };
239
240                (first, second, third)
241            }
242        }
243        // End - impl EulerFromQuaternion
244    };
245}
246
247macro_rules! impl_to_quat {
248    ($t:ty, $quat:ident) => {
249        impl EulerToQuaternion<$t> for EulerRot {
250            type Output = $quat;
251            #[inline(always)]
252            fn new_quat(self, u: $t, v: $t, w: $t) -> $quat {
253                use EulerRot::*;
254                #[inline(always)]
255                fn rot_x(a: $t) -> $quat {
256                    $quat::from_rotation_x(a)
257                }
258                #[inline(always)]
259                fn rot_y(a: $t) -> $quat {
260                    $quat::from_rotation_y(a)
261                }
262                #[inline(always)]
263                fn rot_z(a: $t) -> $quat {
264                    $quat::from_rotation_z(a)
265                }
266                match self {
267                    ZYX => rot_z(u) * rot_y(v) * rot_x(w),
268                    ZXY => rot_z(u) * rot_x(v) * rot_y(w),
269                    YXZ => rot_y(u) * rot_x(v) * rot_z(w),
270                    YZX => rot_y(u) * rot_z(v) * rot_x(w),
271                    XYZ => rot_x(u) * rot_y(v) * rot_z(w),
272                    XZY => rot_x(u) * rot_z(v) * rot_y(w),
273                }
274                .normalize()
275            }
276        }
277        // End - impl EulerToQuaternion
278    };
279}
280
281impl_from_quat!(f32, Quat);
282impl_from_quat!(f64, DQuat);
283impl_to_quat!(f32, Quat);
284impl_to_quat!(f64, DQuat);
glam/euler.rs

glam/
euler.rs
