autd3_core/geometry/
rotation.rs

1use crate::common::Angle;
2
3use super::{UnitQuaternion, Vector3};
4
5#[derive(Debug, Clone, Copy)]
6/// Euler angle (intrinsic)
7pub enum EulerAngle {
8    /// x-y-z euler angle.
9    XYZ(Angle, Angle, Angle),
10    /// x-z-y euler angle.
11    XZY(Angle, Angle, Angle),
12    /// y-x-z euler angle.
13    YXZ(Angle, Angle, Angle),
14    /// y-z-x euler angle.
15    YZX(Angle, Angle, Angle),
16    /// z-x-y euler angle.
17    ZXY(Angle, Angle, Angle),
18    /// z-y-x euler angle.
19    ZYX(Angle, Angle, Angle),
20    /// x-y-x euler angle.
21    XYX(Angle, Angle, Angle),
22    /// x-z-x euler angle.
23    XZX(Angle, Angle, Angle),
24    /// y-x-y euler angle.
25    YXY(Angle, Angle, Angle),
26    /// y-z-y euler angle.
27    YZY(Angle, Angle, Angle),
28    /// z-x-z euler angle.
29    ZXZ(Angle, Angle, Angle),
30    /// z-y-z euler angle.
31    ZYZ(Angle, Angle, Angle),
32}
33
34impl EulerAngle {
35    /// The rotation identity.
36    #[must_use]
37    pub const fn identity() -> Self {
38        Self::XYZ(Angle::ZERO, Angle::ZERO, Angle::ZERO)
39    }
40}
41
42impl From<EulerAngle> for UnitQuaternion {
43    fn from(angle: EulerAngle) -> Self {
44        match angle {
45            EulerAngle::XYZ(first, second, third) => {
46                UnitQuaternion::from_axis_angle(&Vector3::x_axis(), first.radian())
47                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), second.radian())
48                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), third.radian())
49            }
50            EulerAngle::XZY(first, second, third) => {
51                UnitQuaternion::from_axis_angle(&Vector3::x_axis(), first.radian())
52                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), second.radian())
53                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), third.radian())
54            }
55            EulerAngle::YXZ(first, second, third) => {
56                UnitQuaternion::from_axis_angle(&Vector3::y_axis(), first.radian())
57                    * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), second.radian())
58                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), third.radian())
59            }
60            EulerAngle::YZX(first, second, third) => {
61                UnitQuaternion::from_axis_angle(&Vector3::y_axis(), first.radian())
62                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), second.radian())
63                    * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), third.radian())
64            }
65            EulerAngle::ZXY(first, second, third) => {
66                UnitQuaternion::from_axis_angle(&Vector3::z_axis(), first.radian())
67                    * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), second.radian())
68                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), third.radian())
69            }
70            EulerAngle::ZYX(first, second, third) => {
71                UnitQuaternion::from_axis_angle(&Vector3::z_axis(), first.radian())
72                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), second.radian())
73                    * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), third.radian())
74            }
75            EulerAngle::XYX(first, second, third) => {
76                UnitQuaternion::from_axis_angle(&Vector3::x_axis(), first.radian())
77                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), second.radian())
78                    * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), third.radian())
79            }
80            EulerAngle::XZX(first, second, third) => {
81                UnitQuaternion::from_axis_angle(&Vector3::x_axis(), first.radian())
82                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), second.radian())
83                    * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), third.radian())
84            }
85            EulerAngle::YXY(first, second, third) => {
86                UnitQuaternion::from_axis_angle(&Vector3::y_axis(), first.radian())
87                    * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), second.radian())
88                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), third.radian())
89            }
90            EulerAngle::YZY(first, second, third) => {
91                UnitQuaternion::from_axis_angle(&Vector3::y_axis(), first.radian())
92                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), second.radian())
93                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), third.radian())
94            }
95            EulerAngle::ZXZ(first, second, third) => {
96                UnitQuaternion::from_axis_angle(&Vector3::z_axis(), first.radian())
97                    * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), second.radian())
98                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), third.radian())
99            }
100            EulerAngle::ZYZ(first, second, third) => {
101                UnitQuaternion::from_axis_angle(&Vector3::z_axis(), first.radian())
102                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), second.radian())
103                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), third.radian())
104            }
105        }
106    }
107}
108
109#[cfg(test)]
110mod tests {
111    use super::*;
112    use crate::common::{PI, deg, rad};
113
114    macro_rules! assert_approx_eq_quat {
115        ($a:expr, $b:expr) => {
116            approx::assert_abs_diff_eq!($a.w, $b.w, epsilon = 1e-3);
117            approx::assert_abs_diff_eq!($a.i, $b.i, epsilon = 1e-3);
118            approx::assert_abs_diff_eq!($a.j, $b.j, epsilon = 1e-3);
119            approx::assert_abs_diff_eq!($a.k, $b.k, epsilon = 1e-3);
120        };
121    }
122
123    #[rstest::rstest]
124    #[case(0., 0. * deg)]
125    #[case(PI / 2., 90. * deg)]
126    #[case(0., 0. * rad)]
127    #[case(PI / 2., PI / 2. * rad)]
128    fn to_radians(#[case] expected: f32, #[case] angle: Angle) {
129        approx::assert_abs_diff_eq!(expected, angle.radian());
130    }
131
132    #[rstest::rstest]
133    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::XYZ(90. * deg, 0. * deg, 0. * deg))]
134    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::XYZ(0. * deg, 90. * deg, 0. * deg))]
135    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::XYZ(0. * deg, 0. * deg, 90. * deg))]
136    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::XYZ(0. * deg, 90. * deg, 90. * deg))]
137    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::XYZ(90. * deg, 90. * deg, 0. * deg))]
138    fn xyz(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
139        let angle: UnitQuaternion = angle.into();
140        assert_approx_eq_quat!(expected, angle);
141    }
142
143    #[rstest::rstest]
144    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::XZY(90. * deg, 0. * deg, 0. * deg))]
145    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::XZY(0. * deg, 90. * deg, 0. * deg))]
146    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::XZY(0. * deg, 0. * deg, 90. * deg))]
147    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::XZY(0. * deg, 90. * deg, 90. * deg))]
148    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::XZY(90. * deg, 90. * deg, 0. * deg))]
149    fn xzy(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
150        let angle: UnitQuaternion = angle.into();
151        assert_approx_eq_quat!(expected, angle);
152    }
153
154    #[rstest::rstest]
155    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::YXZ(90. * deg, 0. * deg, 0. * deg))]
156    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::YXZ(0. * deg, 90. * deg, 0. * deg))]
157    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::YXZ(0. * deg, 0. * deg, 90. * deg))]
158    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::YXZ(0. * deg, 90. * deg, 90. * deg))]
159    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::YXZ(90. * deg, 90. * deg, 0. * deg))]
160    fn yxz(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
161        let angle: UnitQuaternion = angle.into();
162        assert_approx_eq_quat!(expected, angle);
163    }
164
165    #[rstest::rstest]
166    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::YZX(90. * deg, 0. * deg, 0. * deg))]
167    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::YZX(0. * deg, 90. * deg, 0. * deg))]
168    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::YZX(0. * deg, 0. * deg, 90. * deg))]
169    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::YZX(0. * deg, 90. * deg, 90. * deg))]
170    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::YZX(90. * deg, 90. * deg, 0. * deg))]
171    fn yzx(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
172        let angle: UnitQuaternion = angle.into();
173        assert_approx_eq_quat!(expected, angle);
174    }
175
176    #[rstest::rstest]
177    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZXY(90. * deg, 0. * deg, 0. * deg))]
178    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::ZXY(0. * deg, 90. * deg, 0. * deg))]
179    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::ZXY(0. * deg, 0. * deg, 90. * deg))]
180    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::ZXY(0. * deg, 90. * deg, 90. * deg))]
181    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::ZXY(90. * deg, 90. * deg, 0. * deg))]
182    fn zxy(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
183        let angle: UnitQuaternion = angle.into();
184        assert_approx_eq_quat!(expected, angle);
185    }
186
187    #[rstest::rstest]
188    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZYX(90. * deg, 0. * deg, 0. * deg))]
189    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::ZYX(0. * deg, 90. * deg, 0. * deg))]
190    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::ZYX(0. * deg, 0. * deg, 90. * deg))]
191    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::ZYX(0. * deg, 90. * deg, 90. * deg))]
192    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::ZYX(90. * deg, 90. * deg, 0. * deg))]
193    fn zyx(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
194        let angle: UnitQuaternion = angle.into();
195        assert_approx_eq_quat!(expected, angle);
196    }
197
198    #[rstest::rstest]
199    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::XYX(90. * deg, 0. * deg, 0. * deg))]
200    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::XYX(0. * deg, 90. * deg, 0. * deg))]
201    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::XYX(0. * deg, 0. * deg, 90. * deg))]
202    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::XYX(0. * deg, 90. * deg, 90. * deg))]
203    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::XYX(90. * deg, 90. * deg, 0. * deg))]
204    fn xyx(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
205        let angle: UnitQuaternion = angle.into();
206        assert_approx_eq_quat!(expected, angle);
207    }
208
209    #[rstest::rstest]
210    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::XZX(90. * deg, 0. * deg, 0. * deg))]
211    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::XZX(0. * deg, 90. * deg, 0. * deg))]
212    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::XZX(0. * deg, 0. * deg, 90. * deg))]
213    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::XZX(0. * deg, 90. * deg, 90. * deg))]
214    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::XZX(90. * deg, 90. * deg, 0. * deg))]
215    fn xzx(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
216        let angle: UnitQuaternion = angle.into();
217        assert_approx_eq_quat!(expected, angle);
218    }
219
220    #[rstest::rstest]
221    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::YXY(90. * deg, 0. * deg, 0. * deg))]
222    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::YXY(0. * deg, 90. * deg, 0. * deg))]
223    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::YXY(0. * deg, 0. * deg, 90. * deg))]
224    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::YXY(0. * deg, 90. * deg, 90. * deg))]
225    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::YXY(90. * deg, 90. * deg, 0. * deg))]
226    fn yxy(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
227        let angle: UnitQuaternion = angle.into();
228        assert_approx_eq_quat!(expected, angle);
229    }
230
231    #[rstest::rstest]
232    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::YZY(90. * deg, 0. * deg, 0. * deg))]
233    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::YZY(0. * deg, 90. * deg, 0. * deg))]
234    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::YZY(0. * deg, 0. * deg, 90. * deg))]
235    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::YZY(0. * deg, 90. * deg, 90. * deg))]
236    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::YZY(90. * deg, 90. * deg, 0. * deg))]
237    fn yzy(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
238        let angle: UnitQuaternion = angle.into();
239        assert_approx_eq_quat!(expected, angle);
240    }
241
242    #[rstest::rstest]
243    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZXZ(90. * deg, 0. * deg, 0. * deg))]
244    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::ZXZ(0. * deg, 90. * deg, 0. * deg))]
245    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZXZ(0. * deg, 0. * deg, 90. * deg))]
246    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZXZ(0. * deg, 90. * deg, 90. * deg))]
247    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::ZXZ(90. * deg, 90. * deg, 0. * deg))]
248    fn zxz(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
249        let angle: UnitQuaternion = angle.into();
250        assert_approx_eq_quat!(expected, angle);
251    }
252
253    #[rstest::rstest]
254    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZYZ(90. * deg, 0. * deg, 0. * deg))]
255    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::ZYZ(0. * deg, 90. * deg, 0. * deg))]
256    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZYZ(0. * deg, 0. * deg, 90. * deg))]
257    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZYZ(0. * deg, 90. * deg, 90. * deg))]
258    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::ZYZ(90. * deg, 90. * deg, 0. * deg))]
259    fn zyz(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
260        let angle: UnitQuaternion = angle.into();
261        assert_approx_eq_quat!(expected, angle);
262    }
263
264    #[test]
265    fn identity() {
266        let angle: UnitQuaternion = EulerAngle::identity().into();
267        assert_eq!(UnitQuaternion::identity(), angle);
268    }
269}
autd3_core/geometry/rotation.rs

autd3_core/geometry/
rotation.rs
