glam/
euler.rs

1// Based on Ken Shoemake. 1994. Euler angle conversion. Graphics gems IV.  Academic Press
2// Professional, Inc., USA, 222–229.
3#[cfg(feature = "f64")]
4use crate::{DMat3, DMat4, DQuat, DVec3};
5use crate::{Mat3, Mat3A, Mat4, Quat, Vec3, Vec3A, Vec3Swizzles};
6
7/// Euler rotation sequences.
8///
9/// The three elemental rotations may be extrinsic (rotations about the axes xyz of the original
10/// coordinate system, which is assumed to remain motionless), or intrinsic (rotations about the
11/// axes of the rotating coordinate system XYZ, solidary with the moving body, which changes its
12/// orientation after each elemental rotation).
13///
14/// ```
15/// # use glam::{EulerRot, Mat3};
16/// # let i = core::f32::consts::FRAC_PI_2;
17/// # let j = core::f32::consts::FRAC_PI_4;
18/// # let k = core::f32::consts::FRAC_PI_8;
19/// let m_intrinsic = Mat3::from_rotation_x(i) * Mat3::from_rotation_y(j) * Mat3::from_rotation_z(k);
20/// let n_intrinsic = Mat3::from_euler(EulerRot::XYZ, i, j, k);
21/// assert!(m_intrinsic.abs_diff_eq(n_intrinsic, 2e-6));
22///
23/// let m_extrinsic = Mat3::from_rotation_z(k) * Mat3::from_rotation_y(j) * Mat3::from_rotation_x(i);
24/// let n_extrinsic = Mat3::from_euler(EulerRot::XYZEx, i, j, k);
25/// assert!(m_extrinsic.abs_diff_eq(n_extrinsic, 2e-6));
26/// ```
27///
28#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
29pub enum EulerRot {
30    /// Intrinsic three-axis rotation ZYX
31    ZYX,
32    /// Intrinsic three-axis rotation ZXY
33    ZXY,
34    /// Intrinsic three-axis rotation YXZ
35    YXZ,
36    /// Intrinsic three-axis rotation YZX
37    YZX,
38    /// Intrinsic three-axis rotation XYZ
39    XYZ,
40    /// Intrinsic three-axis rotation XZY
41    XZY,
42
43    /// Intrinsic two-axis rotation ZYZ
44    ZYZ,
45    /// Intrinsic two-axis rotation ZXZ
46    ZXZ,
47    /// Intrinsic two-axis rotation YXY
48    YXY,
49    /// Intrinsic two-axis rotation YZY
50    YZY,
51    /// Intrinsic two-axis rotation XYX
52    XYX,
53    /// Intrinsic two-axis rotation XZX
54    XZX,
55
56    /// Extrinsic three-axis rotation ZYX
57    ZYXEx,
58    /// Extrinsic three-axis rotation ZXY
59    ZXYEx,
60    /// Extrinsic three-axis rotation YXZ
61    YXZEx,
62    /// Extrinsic three-axis rotation YZX
63    YZXEx,
64    /// Extrinsic three-axis rotation XYZ
65    XYZEx,
66    /// Extrinsic three-axis rotation XZY
67    XZYEx,
68
69    /// Extrinsic two-axis rotation ZYZ
70    ZYZEx,
71    /// Extrinsic two-axis rotation ZXZ
72    ZXZEx,
73    /// Extrinsic two-axis rotation YXY
74    YXYEx,
75    /// Extrinsic two-axis rotation YZY
76    YZYEx,
77    /// Extrinsic two-axis rotation XYX
78    XYXEx,
79    /// Extrinsic two-axis rotation XZX
80    XZXEx,
81}
82
83impl Default for EulerRot {
84    /// Default `YXZ` as yaw (y-axis), pitch (x-axis), roll (z-axis).
85    fn default() -> Self {
86        Self::YXZ
87    }
88}
89
90pub(crate) trait ToEuler {
91    type Scalar;
92    fn to_euler_angles(self, order: EulerRot) -> (Self::Scalar, Self::Scalar, Self::Scalar);
93}
94
95pub(crate) trait FromEuler {
96    type Scalar;
97    fn from_euler_angles(
98        order: EulerRot,
99        i: Self::Scalar,
100        j: Self::Scalar,
101        k: Self::Scalar,
102    ) -> Self;
103}
104
105#[derive(Copy, Clone, Debug, PartialEq, Eq)]
106enum Axis {
107    X = 0,
108    Y = 1,
109    Z = 2,
110}
111
112#[derive(Copy, Clone, Debug, PartialEq, Eq)]
113enum Parity {
114    Odd = 0,
115    Even = 1,
116}
117
118#[derive(Copy, Clone, Debug, PartialEq, Eq)]
119enum Repeated {
120    No = 0,
121    Yes = 1,
122}
123
124#[derive(Copy, Clone, Debug, PartialEq, Eq)]
125enum Frame {
126    Relative = 0,
127    Static = 1,
128}
129
130#[derive(Copy, Clone, Debug, PartialEq, Eq)]
131struct Order {
132    initial_axis: Axis,
133    parity_even: bool,
134    initial_repeated: bool,
135    frame_static: bool,
136}
137
138impl Order {
139    const fn new(
140        initial_axis: Axis,
141        parity: Parity,
142        initial_repeated: Repeated,
143        frame: Frame,
144    ) -> Self {
145        Self {
146            initial_axis,
147            parity_even: parity as u32 == Parity::Even as u32,
148            initial_repeated: initial_repeated as u32 == Repeated::Yes as u32,
149            frame_static: frame as u32 == Frame::Static as u32,
150        }
151    }
152
153    const fn from_euler(euler: EulerRot) -> Self {
154        match euler {
155            EulerRot::XYZ => Self::new(Axis::X, Parity::Even, Repeated::No, Frame::Static),
156            EulerRot::XYX => Self::new(Axis::X, Parity::Even, Repeated::Yes, Frame::Static),
157            EulerRot::XZY => Self::new(Axis::X, Parity::Odd, Repeated::No, Frame::Static),
158            EulerRot::XZX => Self::new(Axis::X, Parity::Odd, Repeated::Yes, Frame::Static),
159            EulerRot::YZX => Self::new(Axis::Y, Parity::Even, Repeated::No, Frame::Static),
160            EulerRot::YZY => Self::new(Axis::Y, Parity::Even, Repeated::Yes, Frame::Static),
161            EulerRot::YXZ => Self::new(Axis::Y, Parity::Odd, Repeated::No, Frame::Static),
162            EulerRot::YXY => Self::new(Axis::Y, Parity::Odd, Repeated::Yes, Frame::Static),
163            EulerRot::ZXY => Self::new(Axis::Z, Parity::Even, Repeated::No, Frame::Static),
164            EulerRot::ZXZ => Self::new(Axis::Z, Parity::Even, Repeated::Yes, Frame::Static),
165            EulerRot::ZYX => Self::new(Axis::Z, Parity::Odd, Repeated::No, Frame::Static),
166            EulerRot::ZYZ => Self::new(Axis::Z, Parity::Odd, Repeated::Yes, Frame::Static),
167            EulerRot::ZYXEx => Self::new(Axis::X, Parity::Even, Repeated::No, Frame::Relative),
168            EulerRot::XYXEx => Self::new(Axis::X, Parity::Even, Repeated::Yes, Frame::Relative),
169            EulerRot::YZXEx => Self::new(Axis::X, Parity::Odd, Repeated::No, Frame::Relative),
170            EulerRot::XZXEx => Self::new(Axis::X, Parity::Odd, Repeated::Yes, Frame::Relative),
171            EulerRot::XZYEx => Self::new(Axis::Y, Parity::Even, Repeated::No, Frame::Relative),
172            EulerRot::YZYEx => Self::new(Axis::Y, Parity::Even, Repeated::Yes, Frame::Relative),
173            EulerRot::ZXYEx => Self::new(Axis::Y, Parity::Odd, Repeated::No, Frame::Relative),
174            EulerRot::YXYEx => Self::new(Axis::Y, Parity::Odd, Repeated::Yes, Frame::Relative),
175            EulerRot::YXZEx => Self::new(Axis::Z, Parity::Even, Repeated::No, Frame::Relative),
176            EulerRot::ZXZEx => Self::new(Axis::Z, Parity::Even, Repeated::Yes, Frame::Relative),
177            EulerRot::XYZEx => Self::new(Axis::Z, Parity::Odd, Repeated::No, Frame::Relative),
178            EulerRot::ZYZEx => Self::new(Axis::Z, Parity::Odd, Repeated::Yes, Frame::Relative),
179        }
180    }
181
182    const fn next_axis(i: usize) -> usize {
183        (i + 1) % 3
184    }
185
186    const fn prev_axis(i: usize) -> usize {
187        if i > 0 { i - 1 } else { 2 }
188    }
189
190    const fn angle_order(self) -> (usize, usize, usize) {
191        let i = self.initial_axis as usize;
192        let j = if self.parity_even {
193            Self::next_axis(i)
194        } else {
195            Self::prev_axis(i)
196        };
197        let k = if self.parity_even {
198            Self::prev_axis(i)
199        } else {
200            Self::next_axis(i)
201        };
202        (i, j, k)
203    }
204}
205
206macro_rules! impl_mat3_from_euler {
207    ($scalar:ident, $mat3:ident, $vec3:ident) => {
208        impl FromEuler for $mat3 {
209            type Scalar = $scalar;
210            fn from_euler_angles(
211                euler: EulerRot,
212                x: Self::Scalar,
213                y: Self::Scalar,
214                z: Self::Scalar,
215            ) -> Self {
216                use crate::$scalar::math;
217
218                let order = Order::from_euler(euler);
219                let (i, j, k) = order.angle_order();
220
221                let mut angles = if order.frame_static {
222                    $vec3::new(x, y, z)
223                } else {
224                    $vec3::new(z, y, x)
225                };
226
227                // rotation direction is reverse from original paper
228                if order.parity_even {
229                    angles = -angles;
230                }
231
232                let (si, ci) = math::sin_cos(angles.x);
233                let (sj, cj) = math::sin_cos(angles.y);
234                let (sh, ch) = math::sin_cos(angles.z);
235
236                let cc = ci * ch;
237                let cs = ci * sh;
238                let sc = si * ch;
239                let ss = si * sh;
240
241                let mut m = [[0.0; 3]; 3];
242
243                if order.initial_repeated {
244                    m[i][i] = cj;
245                    m[i][j] = sj * si;
246                    m[i][k] = sj * ci;
247                    m[j][i] = sj * sh;
248                    m[j][j] = -cj * ss + cc;
249                    m[j][k] = -cj * cs - sc;
250                    m[k][i] = -sj * ch;
251                    m[k][j] = cj * sc + cs;
252                    m[k][k] = cj * cc - ss;
253                } else {
254                    m[i][i] = cj * ch;
255                    m[i][j] = sj * sc - cs;
256                    m[i][k] = sj * cc + ss;
257                    m[j][i] = cj * sh;
258                    m[j][j] = sj * ss + cc;
259                    m[j][k] = sj * cs - sc;
260                    m[k][i] = -sj;
261                    m[k][j] = cj * si;
262                    m[k][k] = cj * ci;
263                }
264
265                $mat3::from_cols_array_2d(&m)
266            }
267        }
268    };
269}
270
271macro_rules! impl_mat4_from_euler {
272    ($scalar:ident, $mat4:ident, $mat3:ident) => {
273        impl FromEuler for $mat4 {
274            type Scalar = $scalar;
275            fn from_euler_angles(
276                euler: EulerRot,
277                x: Self::Scalar,
278                y: Self::Scalar,
279                z: Self::Scalar,
280            ) -> Self {
281                $mat4::from_mat3($mat3::from_euler_angles(euler, x, y, z))
282            }
283        }
284    };
285}
286
287macro_rules! impl_quat_from_euler {
288    ($scalar:ident, $quat:ident, $vec3:ident) => {
289        impl FromEuler for $quat {
290            type Scalar = $scalar;
291            fn from_euler_angles(
292                euler: EulerRot,
293                x: Self::Scalar,
294                y: Self::Scalar,
295                z: Self::Scalar,
296            ) -> Self {
297                use crate::$scalar::math;
298
299                let order = Order::from_euler(euler);
300                let (i, j, k) = order.angle_order();
301
302                let mut angles = if order.frame_static {
303                    $vec3::new(x, y, z)
304                } else {
305                    $vec3::new(z, y, x)
306                };
307
308                if order.parity_even {
309                    angles.y = -angles.y;
310                }
311
312                let ti = angles.x * 0.5;
313                let tj = angles.y * 0.5;
314                let th = angles.z * 0.5;
315                let (si, ci) = math::sin_cos(ti);
316                let (sj, cj) = math::sin_cos(tj);
317                let (sh, ch) = math::sin_cos(th);
318                let cc = ci * ch;
319                let cs = ci * sh;
320                let sc = si * ch;
321                let ss = si * sh;
322
323                let parity = if !order.parity_even { 1.0 } else { -1.0 };
324
325                let mut a = [0.0; 4];
326
327                if order.initial_repeated {
328                    a[i] = cj * (cs + sc);
329                    a[j] = sj * (cc + ss) * parity;
330                    a[k] = sj * (cs - sc);
331                    a[3] = cj * (cc - ss);
332                } else {
333                    a[i] = cj * sc - sj * cs;
334                    a[j] = (cj * ss + sj * cc) * parity;
335                    a[k] = cj * cs - sj * sc;
336                    a[3] = cj * cc + sj * ss;
337                }
338
339                $quat::from_array(a)
340            }
341        }
342    };
343}
344
345macro_rules! impl_mat3_to_euler {
346    ($scalar:ident, $mat3:ident, $vec3:ident) => {
347        impl ToEuler for $mat3 {
348            type Scalar = $scalar;
349            fn to_euler_angles(
350                self,
351                euler: EulerRot,
352            ) -> (Self::Scalar, Self::Scalar, Self::Scalar) {
353                use crate::$scalar::math;
354
355                let order = Order::from_euler(euler);
356                let (i, j, k) = order.angle_order();
357
358                let mut ea = $vec3::ZERO;
359                if order.initial_repeated {
360                    let sy = math::sqrt(
361                        self.col(i)[j] * self.col(i)[j] + self.col(i)[k] * self.col(i)[k],
362                    );
363                    if (sy > 16.0 * $scalar::EPSILON) {
364                        ea.x = math::atan2(self.col(i)[j], self.col(i)[k]);
365                        ea.y = math::atan2(sy, self.col(i)[i]);
366                        ea.z = math::atan2(self.col(j)[i], -self.col(k)[i]);
367                    } else {
368                        ea.x = math::atan2(-self.col(j)[k], self.col(j)[j]);
369                        ea.y = math::atan2(sy, self.col(i)[i]);
370                    }
371                } else {
372                    let cy = math::sqrt(
373                        self.col(i)[i] * self.col(i)[i] + self.col(j)[i] * self.col(j)[i],
374                    );
375                    if (cy > 16.0 * $scalar::EPSILON) {
376                        ea.x = math::atan2(self.col(k)[j], self.col(k)[k]);
377                        ea.y = math::atan2(-self.col(k)[i], cy);
378                        ea.z = math::atan2(self.col(j)[i], self.col(i)[i]);
379                    } else {
380                        ea.x = math::atan2(-self.col(j)[k], self.col(j)[j]);
381                        ea.y = math::atan2(-self.col(k)[i], cy);
382                    }
383                }
384
385                // reverse rotation angle of original code
386                if order.parity_even {
387                    ea = -ea;
388                }
389
390                if !order.frame_static {
391                    ea = ea.zyx();
392                }
393
394                (ea.x, ea.y, ea.z)
395            }
396        }
397    };
398}
399
400impl_mat3_to_euler!(f32, Mat3, Vec3);
401impl_mat3_from_euler!(f32, Mat3, Vec3);
402impl_mat3_to_euler!(f32, Mat3A, Vec3A);
403impl_mat3_from_euler!(f32, Mat3A, Vec3A);
404impl_mat4_from_euler!(f32, Mat4, Mat3);
405impl_quat_from_euler!(f32, Quat, Vec3);
406
407#[cfg(feature = "f64")]
408impl_mat3_to_euler!(f64, DMat3, DVec3);
409#[cfg(feature = "f64")]
410impl_mat3_from_euler!(f64, DMat3, DVec3);
411#[cfg(feature = "f64")]
412impl_mat4_from_euler!(f64, DMat4, DMat3);
413#[cfg(feature = "f64")]
414impl_quat_from_euler!(f64, DQuat, DVec3);
glam/euler.rs

glam/
euler.rs
