autd3-core 38.1.0

AUTD3 core traits and types
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269

use crate::common::Angle;

use super::{UnitQuaternion, Vector3};

#[derive(Debug, Clone, Copy)]
/// Euler angle (intrinsic)
pub enum EulerAngle {
    /// x-y-z euler angle.
    XYZ(Angle, Angle, Angle),
    /// x-z-y euler angle.
    XZY(Angle, Angle, Angle),
    /// y-x-z euler angle.
    YXZ(Angle, Angle, Angle),
    /// y-z-x euler angle.
    YZX(Angle, Angle, Angle),
    /// z-x-y euler angle.
    ZXY(Angle, Angle, Angle),
    /// z-y-x euler angle.
    ZYX(Angle, Angle, Angle),
    /// x-y-x euler angle.
    XYX(Angle, Angle, Angle),
    /// x-z-x euler angle.
    XZX(Angle, Angle, Angle),
    /// y-x-y euler angle.
    YXY(Angle, Angle, Angle),
    /// y-z-y euler angle.
    YZY(Angle, Angle, Angle),
    /// z-x-z euler angle.
    ZXZ(Angle, Angle, Angle),
    /// z-y-z euler angle.
    ZYZ(Angle, Angle, Angle),
}

impl EulerAngle {
    /// The rotation identity.
    #[must_use]
    pub const fn identity() -> Self {
        Self::XYZ(Angle::ZERO, Angle::ZERO, Angle::ZERO)
    }
}

impl From<EulerAngle> for UnitQuaternion {
    fn from(angle: EulerAngle) -> Self {
        match angle {
            EulerAngle::XYZ(first, second, third) => {
                UnitQuaternion::from_axis_angle(&Vector3::x_axis(), first.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), second.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), third.radian())
            }
            EulerAngle::XZY(first, second, third) => {
                UnitQuaternion::from_axis_angle(&Vector3::x_axis(), first.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), second.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), third.radian())
            }
            EulerAngle::YXZ(first, second, third) => {
                UnitQuaternion::from_axis_angle(&Vector3::y_axis(), first.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), second.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), third.radian())
            }
            EulerAngle::YZX(first, second, third) => {
                UnitQuaternion::from_axis_angle(&Vector3::y_axis(), first.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), second.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), third.radian())
            }
            EulerAngle::ZXY(first, second, third) => {
                UnitQuaternion::from_axis_angle(&Vector3::z_axis(), first.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), second.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), third.radian())
            }
            EulerAngle::ZYX(first, second, third) => {
                UnitQuaternion::from_axis_angle(&Vector3::z_axis(), first.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), second.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), third.radian())
            }
            EulerAngle::XYX(first, second, third) => {
                UnitQuaternion::from_axis_angle(&Vector3::x_axis(), first.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), second.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), third.radian())
            }
            EulerAngle::XZX(first, second, third) => {
                UnitQuaternion::from_axis_angle(&Vector3::x_axis(), first.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), second.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), third.radian())
            }
            EulerAngle::YXY(first, second, third) => {
                UnitQuaternion::from_axis_angle(&Vector3::y_axis(), first.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), second.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), third.radian())
            }
            EulerAngle::YZY(first, second, third) => {
                UnitQuaternion::from_axis_angle(&Vector3::y_axis(), first.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), second.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), third.radian())
            }
            EulerAngle::ZXZ(first, second, third) => {
                UnitQuaternion::from_axis_angle(&Vector3::z_axis(), first.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::x_axis(), second.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), third.radian())
            }
            EulerAngle::ZYZ(first, second, third) => {
                UnitQuaternion::from_axis_angle(&Vector3::z_axis(), first.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::y_axis(), second.radian())
                    * UnitQuaternion::from_axis_angle(&Vector3::z_axis(), third.radian())
            }
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::common::{PI, deg, rad};

    macro_rules! assert_approx_eq_quat {
        ($a:expr, $b:expr) => {
            approx::assert_abs_diff_eq!($a.w, $b.w, epsilon = 1e-3);
            approx::assert_abs_diff_eq!($a.i, $b.i, epsilon = 1e-3);
            approx::assert_abs_diff_eq!($a.j, $b.j, epsilon = 1e-3);
            approx::assert_abs_diff_eq!($a.k, $b.k, epsilon = 1e-3);
        };
    }

    #[rstest::rstest]
    #[case(0., 0. * deg)]
    #[case(PI / 2., 90. * deg)]
    #[case(0., 0. * rad)]
    #[case(PI / 2., PI / 2. * rad)]
    fn to_radians(#[case] expected: f32, #[case] angle: Angle) {
        approx::assert_abs_diff_eq!(expected, angle.radian());
    }

    #[rstest::rstest]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::XYZ(90. * deg, 0. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::XYZ(0. * deg, 90. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::XYZ(0. * deg, 0. * deg, 90. * deg))]
    #[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))]
    #[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))]
    fn xyz(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
        let angle: UnitQuaternion = angle.into();
        assert_approx_eq_quat!(expected, angle);
    }

    #[rstest::rstest]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::XZY(90. * deg, 0. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::XZY(0. * deg, 90. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::XZY(0. * deg, 0. * deg, 90. * deg))]
    #[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))]
    #[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))]
    fn xzy(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
        let angle: UnitQuaternion = angle.into();
        assert_approx_eq_quat!(expected, angle);
    }

    #[rstest::rstest]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::YXZ(90. * deg, 0. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::YXZ(0. * deg, 90. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::YXZ(0. * deg, 0. * deg, 90. * deg))]
    #[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))]
    #[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))]
    fn yxz(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
        let angle: UnitQuaternion = angle.into();
        assert_approx_eq_quat!(expected, angle);
    }

    #[rstest::rstest]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::YZX(90. * deg, 0. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::YZX(0. * deg, 90. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::YZX(0. * deg, 0. * deg, 90. * deg))]
    #[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))]
    #[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))]
    fn yzx(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
        let angle: UnitQuaternion = angle.into();
        assert_approx_eq_quat!(expected, angle);
    }

    #[rstest::rstest]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZXY(90. * deg, 0. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::ZXY(0. * deg, 90. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::ZXY(0. * deg, 0. * deg, 90. * deg))]
    #[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))]
    #[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))]
    fn zxy(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
        let angle: UnitQuaternion = angle.into();
        assert_approx_eq_quat!(expected, angle);
    }

    #[rstest::rstest]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZYX(90. * deg, 0. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::ZYX(0. * deg, 90. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::ZYX(0. * deg, 0. * deg, 90. * deg))]
    #[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))]
    #[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))]
    fn zyx(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
        let angle: UnitQuaternion = angle.into();
        assert_approx_eq_quat!(expected, angle);
    }

    #[rstest::rstest]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::XYX(90. * deg, 0. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::XYX(0. * deg, 90. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::XYX(0. * deg, 0. * deg, 90. * deg))]
    #[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))]
    #[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))]
    fn xyx(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
        let angle: UnitQuaternion = angle.into();
        assert_approx_eq_quat!(expected, angle);
    }

    #[rstest::rstest]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::XZX(90. * deg, 0. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::XZX(0. * deg, 90. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::XZX(0. * deg, 0. * deg, 90. * deg))]
    #[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))]
    #[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))]
    fn xzx(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
        let angle: UnitQuaternion = angle.into();
        assert_approx_eq_quat!(expected, angle);
    }

    #[rstest::rstest]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::YXY(90. * deg, 0. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::YXY(0. * deg, 90. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::YXY(0. * deg, 0. * deg, 90. * deg))]
    #[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))]
    #[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))]
    fn yxy(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
        let angle: UnitQuaternion = angle.into();
        assert_approx_eq_quat!(expected, angle);
    }

    #[rstest::rstest]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::YZY(90. * deg, 0. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::YZY(0. * deg, 90. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::YZY(0. * deg, 0. * deg, 90. * deg))]
    #[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))]
    #[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))]
    fn yzy(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
        let angle: UnitQuaternion = angle.into();
        assert_approx_eq_quat!(expected, angle);
    }

    #[rstest::rstest]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZXZ(90. * deg, 0. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.), EulerAngle::ZXZ(0. * deg, 90. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZXZ(0. * deg, 0. * deg, 90. * deg))]
    #[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))]
    #[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))]
    fn zxz(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
        let angle: UnitQuaternion = angle.into();
        assert_approx_eq_quat!(expected, angle);
    }

    #[rstest::rstest]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZYZ(90. * deg, 0. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 2.), EulerAngle::ZYZ(0. * deg, 90. * deg, 0. * deg))]
    #[case(UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 2.), EulerAngle::ZYZ(0. * deg, 0. * deg, 90. * deg))]
    #[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))]
    #[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))]
    fn zyz(#[case] expected: UnitQuaternion, #[case] angle: EulerAngle) {
        let angle: UnitQuaternion = angle.into();
        assert_approx_eq_quat!(expected, angle);
    }

    #[test]
    fn identity() {
        let angle: UnitQuaternion = EulerAngle::identity().into();
        assert_eq!(UnitQuaternion::identity(), angle);
    }
}