1 #[macro_use] 2 mod support; 3 4 mod euler { 5 use glam::*; 6 use std::ops::RangeInclusive; 7 8 /// Helper to get the 'canonical' version of a `Quat`. We define the canonical of quat `q` as: 9 /// 10 /// * `q`, if q.w > epsilon 11 /// * `-q`, if q.w < -epsilon 12 /// * `(0, 0, 0, 1)` otherwise 13 /// 14 /// The rationale is that q and -q represent the same rotation, and any (_, _, _, 0) represent no rotation at all. 15 trait CanonicalQuat: Copy { canonical(self) -> Self16 fn canonical(self) -> Self; 17 } 18 19 /// Helper to set some alternative epsilons based on the floating point type used 20 trait EulerEpsilon { 21 /// epsilon for comparing quaternion round-tripped through eulers (quat -> euler -> quat) 22 const E_EPS: f32; 23 } 24 25 impl EulerEpsilon for f32 { 26 const E_EPS: f32 = 2e-6; 27 } 28 29 impl EulerEpsilon for f64 { 30 const E_EPS: f32 = 1e-8; 31 } 32 axis_order(order: EulerRot) -> (usize, usize, usize)33 fn axis_order(order: EulerRot) -> (usize, usize, usize) { 34 match order { 35 EulerRot::XYZ => (0, 1, 2), 36 EulerRot::XYX => (0, 1, 0), 37 EulerRot::XZY => (0, 2, 1), 38 EulerRot::XZX => (0, 2, 0), 39 EulerRot::YZX => (1, 2, 0), 40 EulerRot::YZY => (1, 2, 1), 41 EulerRot::YXZ => (1, 0, 2), 42 EulerRot::YXY => (1, 0, 1), 43 EulerRot::ZXY => (2, 0, 1), 44 EulerRot::ZXZ => (2, 0, 2), 45 EulerRot::ZYX => (2, 1, 0), 46 EulerRot::ZYZ => (2, 1, 2), 47 EulerRot::ZYXEx => (2, 1, 0), 48 EulerRot::XYXEx => (0, 1, 0), 49 EulerRot::YZXEx => (1, 2, 0), 50 EulerRot::XZXEx => (0, 2, 0), 51 EulerRot::XZYEx => (0, 2, 1), 52 EulerRot::YZYEx => (1, 2, 1), 53 EulerRot::ZXYEx => (2, 0, 1), 54 EulerRot::YXYEx => (1, 0, 1), 55 EulerRot::YXZEx => (1, 0, 2), 56 EulerRot::ZXZEx => (2, 0, 2), 57 EulerRot::XYZEx => (0, 1, 2), 58 EulerRot::ZYZEx => (2, 1, 2), 59 } 60 } 61 is_intrinsic(order: EulerRot) -> bool62 fn is_intrinsic(order: EulerRot) -> bool { 63 match order { 64 EulerRot::XYZ 65 | EulerRot::XYX 66 | EulerRot::XZY 67 | EulerRot::XZX 68 | EulerRot::YZX 69 | EulerRot::YZY 70 | EulerRot::YXZ 71 | EulerRot::YXY 72 | EulerRot::ZXY 73 | EulerRot::ZXZ 74 | EulerRot::ZYX 75 | EulerRot::ZYZ => true, 76 EulerRot::ZYXEx 77 | EulerRot::XYXEx 78 | EulerRot::YZXEx 79 | EulerRot::XZXEx 80 | EulerRot::XZYEx 81 | EulerRot::YZYEx 82 | EulerRot::ZXYEx 83 | EulerRot::YXYEx 84 | EulerRot::YXZEx 85 | EulerRot::ZXZEx 86 | EulerRot::XYZEx 87 | EulerRot::ZYZEx => false, 88 } 89 } 90 91 mod f32 { deg_to_rad(a: i32, b: i32, c: i32) -> (f32, f32, f32)92 pub fn deg_to_rad(a: i32, b: i32, c: i32) -> (f32, f32, f32) { 93 ( 94 (a as f32).to_radians(), 95 (b as f32).to_radians(), 96 (c as f32).to_radians(), 97 ) 98 } 99 } 100 101 mod f64 { deg_to_rad(a: i32, b: i32, c: i32) -> (f64, f64, f64)102 pub fn deg_to_rad(a: i32, b: i32, c: i32) -> (f64, f64, f64) { 103 ( 104 (a as f64).to_radians(), 105 (b as f64).to_radians(), 106 (c as f64).to_radians(), 107 ) 108 } 109 } 110 test_order_angles<F: Fn(EulerRot, i32, i32, i32)>(order: EulerRot, test: &F)111 fn test_order_angles<F: Fn(EulerRot, i32, i32, i32)>(order: EulerRot, test: &F) { 112 const RANGE: RangeInclusive<i32> = -180..=180; 113 const STEP: usize = 15; 114 for i in RANGE.step_by(STEP) { 115 for j in RANGE.step_by(STEP) { 116 for k in RANGE.step_by(STEP) { 117 test(order, i, j, k); 118 } 119 } 120 } 121 } 122 test_all_orders<F: Fn(EulerRot)>(test: &F)123 fn test_all_orders<F: Fn(EulerRot)>(test: &F) { 124 test(EulerRot::XYZ); 125 test(EulerRot::XZY); 126 test(EulerRot::YZX); 127 test(EulerRot::YXZ); 128 test(EulerRot::ZXY); 129 test(EulerRot::ZYX); 130 131 test(EulerRot::XZX); 132 test(EulerRot::XYX); 133 test(EulerRot::YXY); 134 test(EulerRot::YZY); 135 test(EulerRot::ZYZ); 136 test(EulerRot::ZXZ); 137 138 test(EulerRot::XYZEx); 139 test(EulerRot::XZYEx); 140 test(EulerRot::YZXEx); 141 test(EulerRot::YXZEx); 142 test(EulerRot::ZXYEx); 143 test(EulerRot::ZYXEx); 144 145 test(EulerRot::XZXEx); 146 test(EulerRot::XYXEx); 147 test(EulerRot::YXYEx); 148 test(EulerRot::YZYEx); 149 test(EulerRot::ZYZEx); 150 test(EulerRot::ZXZEx); 151 } 152 153 macro_rules! impl_quat_euler_test { 154 ($quat:ident, $t:ident) => { 155 use super::{ 156 axis_order, is_intrinsic, test_all_orders, test_order_angles, $t::deg_to_rad, 157 CanonicalQuat, EulerEpsilon, 158 }; 159 use glam::{$quat, EulerRot}; 160 161 const AXIS_ANGLE: [fn($t) -> $quat; 3] = [ 162 $quat::from_rotation_x, 163 $quat::from_rotation_y, 164 $quat::from_rotation_z, 165 ]; 166 167 impl CanonicalQuat for $quat { 168 fn canonical(self) -> Self { 169 match self { 170 _ if self.w >= 1e-5 => self, 171 _ if self.w <= -1e-5 => -self, 172 _ => $quat::from_xyzw(0.0, 0.0, 0.0, 1.0), 173 } 174 } 175 } 176 177 fn test_euler(order: EulerRot, a: i32, b: i32, c: i32) { 178 println!( 179 "test_euler: {} {order:?} ({a}, {b}, {c})", 180 stringify!($quat) 181 ); 182 183 let (a, b, c) = deg_to_rad(a, b, c); 184 let m = $quat::from_euler(order, a, b, c); 185 186 let n = { 187 let (i, j, k) = m.to_euler(order); 188 $quat::from_euler(order, i, j, k) 189 }; 190 assert_approx_eq!(m.canonical(), n.canonical(), $t::E_EPS); 191 192 let o = { 193 let (i, j, k) = axis_order(order); 194 if is_intrinsic(order) { 195 AXIS_ANGLE[i](a) * AXIS_ANGLE[j](b) * AXIS_ANGLE[k](c) 196 } else { 197 AXIS_ANGLE[k](c) * AXIS_ANGLE[j](b) * AXIS_ANGLE[i](a) 198 } 199 }; 200 assert_approx_eq!(m.canonical(), o.canonical(), $t::E_EPS); 201 } 202 203 #[test] 204 fn test_all_euler_orders() { 205 let test = |order| test_order_angles(order, &test_euler); 206 test_all_orders(&test); 207 } 208 }; 209 } 210 211 macro_rules! impl_mat_euler_test { 212 ($mat:ident, $t:ident) => { 213 use super::{ 214 axis_order, is_intrinsic, test_all_orders, test_order_angles, $t::deg_to_rad, 215 EulerEpsilon, 216 }; 217 use glam::{$mat, EulerRot}; 218 219 const AXIS_ANGLE: [fn($t) -> $mat; 3] = [ 220 $mat::from_rotation_x, 221 $mat::from_rotation_y, 222 $mat::from_rotation_z, 223 ]; 224 225 fn test_euler(order: EulerRot, a: i32, b: i32, c: i32) { 226 println!("test_euler: {} {order:?} ({a}, {b}, {c})", stringify!($mat)); 227 228 let (a, b, c) = deg_to_rad(a, b, c); 229 let m = $mat::from_euler(order, a, b, c); 230 let n = { 231 let (i, j, k) = m.to_euler(order); 232 $mat::from_euler(order, i, j, k) 233 }; 234 assert_approx_eq!(m, n, $t::E_EPS); 235 236 let o = { 237 let (i, j, k) = axis_order(order); 238 if is_intrinsic(order) { 239 AXIS_ANGLE[i](a) * AXIS_ANGLE[j](b) * AXIS_ANGLE[k](c) 240 } else { 241 AXIS_ANGLE[k](c) * AXIS_ANGLE[j](b) * AXIS_ANGLE[i](a) 242 } 243 }; 244 assert_approx_eq!(m, o, $t::E_EPS); 245 } 246 247 #[test] 248 fn test_all_euler_orders() { 249 let test = |order| test_order_angles(order, &test_euler); 250 test_all_orders(&test); 251 } 252 }; 253 } 254 255 #[test] test_euler_default()256 fn test_euler_default() { 257 assert_eq!(EulerRot::YXZ, EulerRot::default()); 258 } 259 260 mod quat { 261 impl_quat_euler_test!(Quat, f32); 262 } 263 264 mod mat3 { 265 impl_mat_euler_test!(Mat3, f32); 266 } 267 268 mod mat3a { 269 impl_mat_euler_test!(Mat3A, f32); 270 } 271 272 mod mat4 { 273 impl_mat_euler_test!(Mat4, f32); 274 } 275 276 mod dquat { 277 impl_quat_euler_test!(DQuat, f64); 278 } 279 280 mod dmat3 { 281 impl_mat_euler_test!(DMat3, f64); 282 } 283 284 mod dmat4 { 285 impl_mat_euler_test!(DMat4, f64); 286 } 287 } 288