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

autd3_core/geometry/
rotation.rs
