concinnity_core/math/
rotation.rs

1//! The engine's rotation convention, in one place: unit quaternions as
2//! `[x, y, z, w]`, and the YXZ Euler decomposition every authored
3//! `[pitch, yaw, roll]` triple is read and written through.
4//!
5//! Both directions live here rather than with a caller because the two sides
6//! that need them are far apart: the cook turns imported quaternions into
7//! authored angles, and the simulation turns stepped body rotations back into
8//! the same. A second implementation would let those two disagree about what a
9//! rotation means.
10
11use crate::math::{atan2, sin_cos, sqrt};
12
13/// Unit quaternion `(x, y, z, w)` representing a rotation.
14pub type Quat = [f32; 4];
15
16/// Hamilton product `a * b`.
17fn mul(a: Quat, b: Quat) -> Quat {
18    let [ax, ay, az, aw] = a;
19    let [bx, by, bz, bw] = b;
20    [
21        aw * bx + ax * bw + ay * bz - az * by,
22        aw * by - ax * bz + ay * bw + az * bx,
23        aw * bz + ax * by - ay * bx + az * bw,
24        aw * bw - ax * bx - ay * by - az * bz,
25    ]
26}
27
28/// Rotation of `angle_rad` about a canonical axis, `axis` being 0, 1 or 2.
29fn about_axis(axis: usize, angle_rad: f32) -> Quat {
30    let (s, c) = sin_cos(angle_rad * 0.5);
31    let mut q = [0.0, 0.0, 0.0, c];
32    q[axis] = s;
33    q
34}
35
36/// A rotation quaternion scaled to unit length, or the identity when `q` is too
37/// short to have a direction.
38pub fn quat_normalize(q: Quat) -> Quat {
39    let len = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
40    if len < 1e-12 {
41        return [0.0, 0.0, 0.0, 1.0];
42    }
43    [q[0] / len, q[1] / len, q[2] / len, q[3] / len]
44}
45
46/// The rotation quaternion for engine Euler degrees `[pitch, yaw, roll]`,
47/// applied yaw then pitch then roll.
48pub fn quat_from_euler_yxz_deg(euler_deg: [f32; 3]) -> Quat {
49    let [pitch, yaw, roll] = euler_deg;
50    mul(
51        mul(
52            about_axis(1, yaw.to_radians()),
53            about_axis(0, pitch.to_radians()),
54        ),
55        about_axis(2, roll.to_radians()),
56    )
57}
58
59/// Engine Euler degrees `[pitch, yaw, roll]` for a rotation quaternion, which
60/// need not be normalised.
61///
62/// The decomposition is lossy at +-90 degrees of pitch, where yaw and roll fold
63/// into one angle: the whole rotation is reported as yaw.
64pub fn euler_yxz_deg_from_quat(q: Quat) -> [f32; 3] {
65    let [x, y, z, w] = quat_normalize(q);
66    // The column-major rotation-matrix entries the YXZ decomposition reads.
67    let m21 = 2.0 * (y * z - w * x);
68    let m20 = 2.0 * (x * z + w * y);
69    let m22 = 1.0 - 2.0 * (x * x + y * y);
70    let m01 = 2.0 * (x * y + w * z);
71    let m11 = 1.0 - 2.0 * (x * x + z * z);
72    let m10 = 2.0 * (x * y - w * z);
73    let m00 = 1.0 - 2.0 * (y * y + z * z);
74
75    let sp = (-m21).clamp(-1.0, 1.0);
76    // cos(pitch) is taken straight from the matrix (the column-2 length in the
77    // XZ plane) rather than `sqrt(1 - sp*sp)`, which loses nearly all its
78    // precision to catastrophic cancellation as pitch approaches +-90 degrees.
79    let cp = sqrt(m20 * m20 + m22 * m22);
80    let pitch = atan2(sp, cp);
81    let (yaw, roll) = if cp > 1e-4 {
82        (atan2(m20, m22), atan2(m01, m11))
83    } else {
84        (atan2(sp * m10, m00), 0.0)
85    };
86    [pitch.to_degrees(), yaw.to_degrees(), roll.to_degrees()]
87}
88
89#[cfg(test)]
90mod tests {
91    use super::*;
92
93    fn close(a: [f32; 3], b: [f32; 3], tol: f32) -> bool {
94        (0..3).all(|i| (a[i] - b[i]).abs() < tol)
95    }
96
97    #[test]
98    fn euler_round_trips_away_from_gimbal_lock() {
99        for e in [
100            [0.0, 0.0, 0.0],
101            [12.0, -34.0, 56.0],
102            [-80.0, 170.0, -170.0],
103            [45.0, 90.0, -45.0],
104        ] {
105            let got = euler_yxz_deg_from_quat(quat_from_euler_yxz_deg(e));
106            assert!(close(got, e, 1e-2), "{e:?} round-tripped to {got:?}");
107        }
108    }
109
110    #[test]
111    fn pitch_at_gimbal_lock_pins_roll_to_zero() {
112        for pitch in [-90.0, 90.0] {
113            let got = euler_yxz_deg_from_quat(quat_from_euler_yxz_deg([pitch, 30.0, 40.0]));
114            assert!((got[0] - pitch).abs() < 1e-2, "pitch became {}", got[0]);
115            assert_eq!(got[2], 0.0, "roll should fold into yaw");
116        }
117    }
118
119    #[test]
120    fn a_degenerate_quaternion_normalises_to_the_identity() {
121        assert_eq!(quat_normalize([0.0; 4]), [0.0, 0.0, 0.0, 1.0]);
122        assert_eq!(euler_yxz_deg_from_quat([0.0; 4]), [0.0, 0.0, 0.0]);
123    }
124
125    #[test]
126    fn the_quaternion_built_is_already_unit_length() {
127        for e in [[0.0, 0.0, 0.0], [12.0, -34.0, 56.0], [-89.0, 179.0, 61.0]] {
128            let q = quat_from_euler_yxz_deg(e);
129            let len2 = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
130            assert!((len2 - 1.0).abs() < 1e-5, "{e:?} gave length^2 {len2}");
131        }
132    }
133}
concinnity_core/math/rotation.rs

concinnity_core/math/
rotation.rs
