1 /*
2 Conversion from quaternions to Euler rotation sequences.
3 
4 From: http://bediyap.com/programming/convert-quaternion-to-euler-rotations/
5 */
6 
7 use crate::{DQuat, Quat};
8 
9 /// Euler rotation sequences.
10 ///
11 /// The angles are applied starting from the right.
12 /// E.g. XYZ will first apply the z-axis rotation.
13 ///
14 /// YXZ can be used for yaw (y-axis), pitch (x-axis), roll (z-axis).
15 #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
16 pub enum EulerRot {
17     /// Intrinsic three-axis rotation ZYX
18     ZYX,
19     /// Intrinsic three-axis rotation ZXY
20     ZXY,
21     /// Intrinsic three-axis rotation YXZ
22     YXZ,
23     /// Intrinsic three-axis rotation YZX
24     YZX,
25     /// Intrinsic three-axis rotation XYZ
26     XYZ,
27     /// Intrinsic three-axis rotation XZY
28     XZY,
29 }
30 
31 impl Default for EulerRot {
32     /// Default `YXZ` as yaw (y-axis), pitch (x-axis), roll (z-axis).
default() -> Self33     fn default() -> Self {
34         Self::YXZ
35     }
36 }
37 
38 /// Conversion from quaternion to euler angles.
39 pub(crate) trait EulerFromQuaternion<Q: Copy>: Sized + Copy {
40     type Output;
41     /// Compute the angle of the first axis (X-x-x)
first(self, q: Q) -> Self::Output42     fn first(self, q: Q) -> Self::Output;
43     /// Compute then angle of the second axis (x-X-x)
second(self, q: Q) -> Self::Output44     fn second(self, q: Q) -> Self::Output;
45     /// Compute then angle of the third axis (x-x-X)
third(self, q: Q) -> Self::Output46     fn third(self, q: Q) -> Self::Output;
47 
48     /// Compute all angles of a rotation in the notation order
convert_quat(self, q: Q) -> (Self::Output, Self::Output, Self::Output)49     fn convert_quat(self, q: Q) -> (Self::Output, Self::Output, Self::Output);
50 
51     #[doc(hidden)]
sine_theta(self, q: Q) -> Self::Output52     fn sine_theta(self, q: Q) -> Self::Output;
53 }
54 
55 /// Conversion from euler angles to quaternion.
56 pub(crate) trait EulerToQuaternion<T>: Copy {
57     type Output;
58     /// Create the rotation quaternion for the three angles of this euler rotation sequence.
new_quat(self, u: T, v: T, w: T) -> Self::Output59     fn new_quat(self, u: T, v: T, w: T) -> Self::Output;
60 }
61 
62 macro_rules! impl_from_quat {
63     ($t:ident, $quat:ident) => {
64         impl EulerFromQuaternion<$quat> for EulerRot {
65             type Output = $t;
66 
67             fn sine_theta(self, q: $quat) -> $t {
68                 use EulerRot::*;
69                 match self {
70                     ZYX => -2.0 * (q.x * q.z - q.w * q.y),
71                     ZXY => 2.0 * (q.y * q.z + q.w * q.x),
72                     YXZ => -2.0 * (q.y * q.z - q.w * q.x),
73                     YZX => 2.0 * (q.x * q.y + q.w * q.z),
74                     XYZ => 2.0 * (q.x * q.z + q.w * q.y),
75                     XZY => -2.0 * (q.x * q.y - q.w * q.z),
76                 }
77                 .clamp(-1.0, 1.0)
78             }
79 
80             fn first(self, q: $quat) -> $t {
81                 use crate::$t::math;
82                 use EulerRot::*;
83 
84                 let sine_theta = self.sine_theta(q);
85                 if math::abs(sine_theta) > 0.99999 {
86                     let scale = 2.0 * math::signum(sine_theta);
87 
88                     match self {
89                         ZYX => scale * math::atan2(-q.x, q.w),
90                         ZXY => scale * math::atan2(q.y, q.w),
91                         YXZ => scale * math::atan2(-q.z, q.w),
92                         YZX => scale * math::atan2(q.x, q.w),
93                         XYZ => scale * math::atan2(q.z, q.w),
94                         XZY => scale * math::atan2(-q.y, q.w),
95                     }
96                 } else {
97                     match self {
98                         ZYX => math::atan2(
99                             2.0 * (q.x * q.y + q.w * q.z),
100                             q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
101                         ),
102                         ZXY => math::atan2(
103                             -2.0 * (q.x * q.y - q.w * q.z),
104                             q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
105                         ),
106                         YXZ => math::atan2(
107                             2.0 * (q.x * q.z + q.w * q.y),
108                             q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
109                         ),
110                         YZX => math::atan2(
111                             -2.0 * (q.x * q.z - q.w * q.y),
112                             q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
113                         ),
114                         XYZ => math::atan2(
115                             -2.0 * (q.y * q.z - q.w * q.x),
116                             q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
117                         ),
118                         XZY => math::atan2(
119                             2.0 * (q.y * q.z + q.w * q.x),
120                             q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
121                         ),
122                     }
123                 }
124             }
125 
126             fn second(self, q: $quat) -> $t {
127                 use crate::$t::math;
128                 math::asin(self.sine_theta(q))
129             }
130 
131             fn third(self, q: $quat) -> $t {
132                 use crate::$t::math;
133                 use EulerRot::*;
134                 if math::abs(self.sine_theta(q)) > 0.99999 {
135                     0.0
136                 } else {
137                     match self {
138                         ZYX => math::atan2(
139                             2.0 * (q.y * q.z + q.w * q.x),
140                             q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
141                         ),
142                         ZXY => math::atan2(
143                             -2.0 * (q.x * q.z - q.w * q.y),
144                             q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
145                         ),
146                         YXZ => math::atan2(
147                             2.0 * (q.x * q.y + q.w * q.z),
148                             q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
149                         ),
150                         YZX => math::atan2(
151                             -2.0 * (q.y * q.z - q.w * q.x),
152                             q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
153                         ),
154                         XYZ => math::atan2(
155                             -2.0 * (q.x * q.y - q.w * q.z),
156                             q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
157                         ),
158                         XZY => math::atan2(
159                             2.0 * (q.x * q.z + q.w * q.y),
160                             q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
161                         ),
162                     }
163                 }
164             }
165 
166             fn convert_quat(self, q: $quat) -> ($t, $t, $t) {
167                 use crate::$t::math;
168                 use EulerRot::*;
169 
170                 let sine_theta = self.sine_theta(q);
171                 let second = math::asin(sine_theta);
172 
173                 if math::abs(sine_theta) > 0.99999 {
174                     let scale = 2.0 * math::signum(sine_theta);
175 
176                     return match self {
177                         ZYX => (scale * math::atan2(-q.x, q.w), second, 0.0),
178                         ZXY => (scale * math::atan2(q.y, q.w), second, 0.0),
179                         YXZ => (scale * math::atan2(-q.z, q.w), second, 0.0),
180                         YZX => (scale * math::atan2(q.x, q.w), second, 0.0),
181                         XYZ => (scale * math::atan2(q.z, q.w), second, 0.0),
182                         XZY => (scale * math::atan2(-q.y, q.w), second, 0.0),
183                     };
184                 }
185 
186                 let first = match self {
187                     ZYX => math::atan2(
188                         2.0 * (q.x * q.y + q.w * q.z),
189                         q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
190                     ),
191                     ZXY => math::atan2(
192                         -2.0 * (q.x * q.y - q.w * q.z),
193                         q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
194                     ),
195                     YXZ => math::atan2(
196                         2.0 * (q.x * q.z + q.w * q.y),
197                         q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
198                     ),
199                     YZX => math::atan2(
200                         -2.0 * (q.x * q.z - q.w * q.y),
201                         q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
202                     ),
203                     XYZ => math::atan2(
204                         -2.0 * (q.y * q.z - q.w * q.x),
205                         q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
206                     ),
207                     XZY => math::atan2(
208                         2.0 * (q.y * q.z + q.w * q.x),
209                         q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
210                     ),
211                 };
212 
213                 let third = match self {
214                     ZYX => math::atan2(
215                         2.0 * (q.y * q.z + q.w * q.x),
216                         q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
217                     ),
218                     ZXY => math::atan2(
219                         -2.0 * (q.x * q.z - q.w * q.y),
220                         q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
221                     ),
222                     YXZ => math::atan2(
223                         2.0 * (q.x * q.y + q.w * q.z),
224                         q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
225                     ),
226                     YZX => math::atan2(
227                         -2.0 * (q.y * q.z - q.w * q.x),
228                         q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
229                     ),
230                     XYZ => math::atan2(
231                         -2.0 * (q.x * q.y - q.w * q.z),
232                         q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
233                     ),
234                     XZY => math::atan2(
235                         2.0 * (q.x * q.z + q.w * q.y),
236                         q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
237                     ),
238                 };
239 
240                 (first, second, third)
241             }
242         }
243         // End - impl EulerFromQuaternion
244     };
245 }
246 
247 macro_rules! impl_to_quat {
248     ($t:ty, $quat:ident) => {
249         impl EulerToQuaternion<$t> for EulerRot {
250             type Output = $quat;
251             #[inline(always)]
252             fn new_quat(self, u: $t, v: $t, w: $t) -> $quat {
253                 use EulerRot::*;
254                 #[inline(always)]
255                 fn rot_x(a: $t) -> $quat {
256                     $quat::from_rotation_x(a)
257                 }
258                 #[inline(always)]
259                 fn rot_y(a: $t) -> $quat {
260                     $quat::from_rotation_y(a)
261                 }
262                 #[inline(always)]
263                 fn rot_z(a: $t) -> $quat {
264                     $quat::from_rotation_z(a)
265                 }
266                 match self {
267                     ZYX => rot_z(u) * rot_y(v) * rot_x(w),
268                     ZXY => rot_z(u) * rot_x(v) * rot_y(w),
269                     YXZ => rot_y(u) * rot_x(v) * rot_z(w),
270                     YZX => rot_y(u) * rot_z(v) * rot_x(w),
271                     XYZ => rot_x(u) * rot_y(v) * rot_z(w),
272                     XZY => rot_x(u) * rot_z(v) * rot_y(w),
273                 }
274                 .normalize()
275             }
276         }
277         // End - impl EulerToQuaternion
278     };
279 }
280 
281 impl_from_quat!(f32, Quat);
282 impl_from_quat!(f64, DQuat);
283 impl_to_quat!(f32, Quat);
284 impl_to_quat!(f64, DQuat);
285