autd3_core/geometry/
rotation.rs

1use crate::defined::Angle;
2
3use super::{UnitQuaternion, Vector3};
4
5use paste::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::defined::{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    #[test]
105    #[case(0., 0. * deg)]
106    #[case(PI / 2., 90. * deg)]
107    #[case(0., 0. * rad)]
108    #[case(PI / 2., PI / 2. * rad)]
109    fn to_radians(#[case] expected: f32, #[case] angle: Angle) {
110        approx::assert_abs_diff_eq!(expected, angle.radian());
111    }
112
113    #[rstest::rstest]
114    #[test]
115    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::XYZ(90. * deg, 0. * deg, 0. * deg))]
116    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::XYZ(0. * deg, 90. * deg, 0. * deg))]
117    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::XYZ(0. * deg, 0. * deg, 90. * deg))]
118    #[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))]
119    #[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))]
120    fn xyz_intrinsic(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
121        let angle: UnitQuaternion = angle.into();
122        assert_approx_eq_quat!(expected, angle);
123    }
124
125    #[rstest::rstest]
126    #[test]
127    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZYZ(90. * deg, 0. * deg, 0. * deg))]
128    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::ZYZ(0. * deg, 90. * deg, 0. * deg))]
129    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZYZ(0. * deg, 0. * deg, 90. * deg))]
130    #[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))]
131    #[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))]
132    fn zyz_intrinsic(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
133        let angle: UnitQuaternion = angle.into();
134        assert_approx_eq_quat!(expected, angle);
135    }
136
137    #[rstest::rstest]
138    #[test]
139    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngleExtrinsic::XYZ(90. * deg, 0. * deg, 0. * deg))]
140    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngleExtrinsic::XYZ(0. * deg, 90. * deg, 0. * deg))]
141    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngleExtrinsic::XYZ(0. * deg, 0. * deg, 90. * deg))]
142    #[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))]
143    #[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))]
144    fn xyz_extrinsic(#[case] expected: UnitQuaternion, #[case] angle: EulerAngleExtrinsic) {
145        let angle: UnitQuaternion = angle.into();
146        assert_approx_eq_quat!(expected, angle);
147    }
148
149    #[rstest::rstest]
150    #[test]
151    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngleExtrinsic::ZYZ(90. * deg, 0. * deg, 0. * deg))]
152    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngleExtrinsic::ZYZ(0. * deg, 90. * deg, 0. * deg))]
153    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngleExtrinsic::ZYZ(0. * deg, 0. * deg, 90. * deg))]
154    #[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))]
155    #[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))]
156    fn zyz_extrinsic(#[case] expected: UnitQuaternion, #[case] angle: EulerAngleExtrinsic) {
157        let angle: UnitQuaternion = angle.into();
158        assert_approx_eq_quat!(expected, angle);
159    }
160
161    #[rstest::rstest]
162    #[case(EulerAngleExtrinsic::identity())]
163    #[case(EulerAngleIntrinsic::identity())]
164    #[test]
165    fn identity(#[case] angle: impl Into<UnitQuaternion>) {
166        let angle: UnitQuaternion = angle.into();
167        assert_eq!(UnitQuaternion::identity(), angle);
168    }
169}
autd3_core/geometry/rotation.rs

autd3_core/geometry/
rotation.rs
