autd3_core/geometry/
rotation.rs

1use crate::common::Angle;
2
3use super::{UnitQuaternion, Vector3};
4
5use pastey::paste;
6
7macro_rules! make_euler_angle_intrinsic {
8    ($({$first:ident, $second:ident, $third:ident}),*) => {
9        paste! {
10            #[derive(Debug, Clone, Copy)]
11            /// Euler angle (intrinsic)
12            pub enum EulerAngleIntrinsic {
13                $(
14                    #[doc = stringify!($first-$second-$third)]
15                    #[doc = "euler angle."]
16                    [<$first:upper $second:upper $third:upper>](Angle, Angle, Angle),
17                )*
18            }
19
20            impl EulerAngleIntrinsic {
21                /// The rotation identity.
22                #[must_use]
23                pub const fn identity() -> Self {
24                    Self::XYZ(Angle::ZERO, Angle::ZERO, Angle::ZERO)
25                }
26            }
27
28            impl From<EulerAngleIntrinsic> for UnitQuaternion {
29                fn from(angle: EulerAngleIntrinsic) -> Self {
30                    match angle {
31                        $(
32                            EulerAngleIntrinsic::[<$first:upper $second:upper $third:upper>](first, second, third) => {
33                                UnitQuaternion::from_axis_angle(&Vector3::[<$first _axis>](), first.radian())
34                                    * UnitQuaternion::from_axis_angle(&Vector3::[<$second _axis>](), second.radian())
35                                    * UnitQuaternion::from_axis_angle(&Vector3::[<$third _axis>](), third.radian())
36                            }
37                        )*
38                    }
39                }
40            }
41        }
42    }
43}
44
45macro_rules! make_euler_angle_extrinsic {
46    ($({$first:ident, $second:ident, $third:ident}),*) => {
47        paste! {
48            #[derive(Debug, Clone, Copy)]
49            /// Euler angle (extrinsic)
50            pub enum EulerAngleExtrinsic {
51                $(
52                    #[doc = stringify!($first-$second-$third)]
53                    #[doc = "euler angle."]
54                    [<$first:upper $second:upper $third:upper>](Angle, Angle, Angle),
55                )*
56            }
57
58            impl EulerAngleExtrinsic {
59                /// The rotation identity.
60                #[must_use]
61                pub const fn identity() -> Self {
62                    Self::XYZ(Angle::ZERO, Angle::ZERO, Angle::ZERO)
63                }
64            }
65
66            impl From<EulerAngleExtrinsic> for UnitQuaternion {
67                fn from(angle: EulerAngleExtrinsic) -> Self {
68                    match angle {
69                        $(
70                            EulerAngleExtrinsic::[<$first:upper $second:upper $third:upper>](first, second, third) => {
71                                UnitQuaternion::from_axis_angle(&Vector3::[<$third _axis>](), third.radian())
72                                    * UnitQuaternion::from_axis_angle(&Vector3::[<$second _axis>](), second.radian())
73                                    * UnitQuaternion::from_axis_angle(&Vector3::[<$first _axis>](), first.radian())
74                            }
75                        )*
76                    }
77                }
78            }
79        }
80    }
81}
82
83make_euler_angle_intrinsic!({x, y, z}, {x, z, y}, {y, x, z}, {y, z, x}, {z, x, y}, {z, y, x}, {x, y, x}, {x, z, x}, {y, x, y}, {y, z, y}, {z, x, z}, {z, y, z});
84make_euler_angle_extrinsic!({x, y, z}, {x, z, y}, {y, x, z}, {y, z, x}, {z, x, y}, {z, y, x}, {x, y, x}, {x, z, x}, {y, x, y}, {y, z, y}, {z, x, z}, {z, y, z});
85
86/// Euler angle (intrinsic)
87pub type EulerAngle = EulerAngleIntrinsic;
88
89#[cfg(test)]
90mod tests {
91    use super::*;
92    use crate::common::{PI, deg, rad};
93
94    macro_rules! assert_approx_eq_quat {
95        ($a:expr, $b:expr) => {
96            approx::assert_abs_diff_eq!($a.w, $b.w, epsilon = 1e-3);
97            approx::assert_abs_diff_eq!($a.i, $b.i, epsilon = 1e-3);
98            approx::assert_abs_diff_eq!($a.j, $b.j, epsilon = 1e-3);
99            approx::assert_abs_diff_eq!($a.k, $b.k, epsilon = 1e-3);
100        };
101    }
102
103    #[rstest::rstest]
104    #[case(0., 0. * deg)]
105    #[case(PI / 2., 90. * deg)]
106    #[case(0., 0. * rad)]
107    #[case(PI / 2., PI / 2. * rad)]
108    fn to_radians(#[case] expected: f32, #[case] angle: Angle) {
109        approx::assert_abs_diff_eq!(expected, angle.radian());
110    }
111
112    #[rstest::rstest]
113    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::XYZ(90. * deg, 0. * deg, 0. * deg))]
114    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::XYZ(0. * deg, 90. * deg, 0. * deg))]
115    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::XYZ(0. * deg, 0. * deg, 90. * deg))]
116    #[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))]
117    #[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))]
118    fn xyz_intrinsic(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
119        let angle: UnitQuaternion = angle.into();
120        assert_approx_eq_quat!(expected, angle);
121    }
122
123    #[rstest::rstest]
124    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZYZ(90. * deg, 0. * deg, 0. * deg))]
125    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::ZYZ(0. * deg, 90. * deg, 0. * deg))]
126    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZYZ(0. * deg, 0. * deg, 90. * deg))]
127    #[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))]
128    #[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))]
129    fn zyz_intrinsic(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
130        let angle: UnitQuaternion = angle.into();
131        assert_approx_eq_quat!(expected, angle);
132    }
133
134    #[rstest::rstest]
135    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngleExtrinsic::XYZ(90. * deg, 0. * deg, 0. * deg))]
136    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngleExtrinsic::XYZ(0. * deg, 90. * deg, 0. * deg))]
137    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngleExtrinsic::XYZ(0. * deg, 0. * deg, 90. * deg))]
138    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngleExtrinsic::XYZ(0. * deg, 90. * deg, 90. * deg))]
139    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngleExtrinsic::XYZ(90. * deg, 90. * deg, 0. * deg))]
140    fn xyz_extrinsic(#[case] expected: UnitQuaternion, #[case] angle: EulerAngleExtrinsic) {
141        let angle: UnitQuaternion = angle.into();
142        assert_approx_eq_quat!(expected, angle);
143    }
144
145    #[rstest::rstest]
146    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngleExtrinsic::ZYZ(90. * deg, 0. * deg, 0. * deg))]
147    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngleExtrinsic::ZYZ(0. * deg, 90. * deg, 0. * deg))]
148    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngleExtrinsic::ZYZ(0. * deg, 0. * deg, 90. * deg))]
149    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngleExtrinsic::ZYZ(0. * deg, 90. * deg, 90. * deg))]
150    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.) * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngleExtrinsic::ZYZ(90. * deg, 90. * deg, 0. * deg))]
151    fn zyz_extrinsic(#[case] expected: UnitQuaternion, #[case] angle: EulerAngleExtrinsic) {
152        let angle: UnitQuaternion = angle.into();
153        assert_approx_eq_quat!(expected, angle);
154    }
155
156    #[rstest::rstest]
157    #[case(EulerAngleExtrinsic::identity())]
158    #[case(EulerAngleIntrinsic::identity())]
159    #[test]
160    fn identity(#[case] angle: impl Into<UnitQuaternion>) {
161        let angle: UnitQuaternion = angle.into();
162        assert_eq!(UnitQuaternion::identity(), angle);
163    }
164}
autd3_core/geometry/rotation.rs

autd3_core/geometry/
rotation.rs
