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/// The rotation quaternion for `angle_rad` about `axis`, which need not be
37/// normalised. An axis too short to have a direction yields the identity.
38pub fn quat_from_axis_angle(axis: [f32; 3], angle_rad: f32) -> Quat {
39    let len = sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]);
40    if len < 1e-12 {
41        return [0.0, 0.0, 0.0, 1.0];
42    }
43    let (s, c) = sin_cos(angle_rad * 0.5);
44    let k = s / len;
45    [axis[0] * k, axis[1] * k, axis[2] * k, c]
46}
47
48/// A rotation quaternion scaled to unit length, or the identity when `q` is too
49/// short to have a direction.
50pub fn quat_normalize(q: Quat) -> Quat {
51    let len = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
52    if len < 1e-12 {
53        return [0.0, 0.0, 0.0, 1.0];
54    }
55    [q[0] / len, q[1] / len, q[2] / len, q[3] / len]
56}
57
58/// The rotation quaternion for engine Euler degrees `[pitch, yaw, roll]`,
59/// applied yaw then pitch then roll.
60pub fn quat_from_euler_yxz_deg(euler_deg: [f32; 3]) -> Quat {
61    let [pitch, yaw, roll] = euler_deg;
62    mul(
63        mul(
64            about_axis(1, yaw.to_radians()),
65            about_axis(0, pitch.to_radians()),
66        ),
67        about_axis(2, roll.to_radians()),
68    )
69}
70
71/// Engine Euler degrees `[pitch, yaw, roll]` for a rotation quaternion, which
72/// need not be normalised.
73///
74/// The decomposition is lossy at +-90 degrees of pitch, where yaw and roll fold
75/// into one angle: the whole rotation is reported as yaw.
76pub fn euler_yxz_deg_from_quat(q: Quat) -> [f32; 3] {
77    let [x, y, z, w] = quat_normalize(q);
78    // The column-major rotation-matrix entries the YXZ decomposition reads.
79    let m21 = 2.0 * (y * z - w * x);
80    let m20 = 2.0 * (x * z + w * y);
81    let m22 = 1.0 - 2.0 * (x * x + y * y);
82    let m01 = 2.0 * (x * y + w * z);
83    let m11 = 1.0 - 2.0 * (x * x + z * z);
84    let m10 = 2.0 * (x * y - w * z);
85    let m00 = 1.0 - 2.0 * (y * y + z * z);
86
87    let sp = (-m21).clamp(-1.0, 1.0);
88    // cos(pitch) is taken straight from the matrix (the column-2 length in the
89    // XZ plane) rather than `sqrt(1 - sp*sp)`, which loses nearly all its
90    // precision to catastrophic cancellation as pitch approaches +-90 degrees.
91    let cp = sqrt(m20 * m20 + m22 * m22);
92    let pitch = atan2(sp, cp);
93    let (yaw, roll) = if cp > 1e-4 {
94        (atan2(m20, m22), atan2(m01, m11))
95    } else {
96        (atan2(sp * m10, m00), 0.0)
97    };
98    [pitch.to_degrees(), yaw.to_degrees(), roll.to_degrees()]
99}
100
101#[cfg(test)]
102mod tests {
103    use super::*;
104
105    fn close(a: [f32; 3], b: [f32; 3], tol: f32) -> bool {
106        (0..3).all(|i| (a[i] - b[i]).abs() < tol)
107    }
108
109    // The axis-angle quaternion agrees with the canonical single-axis one, and
110    // a zero axis has no direction to turn about.
111    #[test]
112    fn axis_angle_matches_the_canonical_axis_rotation() {
113        let want = about_axis(1, 0.7);
114        let got = quat_from_axis_angle([0.0, 3.0, 0.0], 0.7);
115        assert!((0..4).all(|i| (got[i] - want[i]).abs() < 1e-6), "{got:?}");
116        assert_eq!(quat_from_axis_angle([0.0; 3], 1.0), [0.0, 0.0, 0.0, 1.0]);
117    }
118
119    #[test]
120    fn euler_round_trips_away_from_gimbal_lock() {
121        for e in [
122            [0.0, 0.0, 0.0],
123            [12.0, -34.0, 56.0],
124            [-80.0, 170.0, -170.0],
125            [45.0, 90.0, -45.0],
126        ] {
127            let got = euler_yxz_deg_from_quat(quat_from_euler_yxz_deg(e));
128            assert!(close(got, e, 1e-2), "{e:?} round-tripped to {got:?}");
129        }
130    }
131
132    #[test]
133    fn pitch_at_gimbal_lock_pins_roll_to_zero() {
134        for pitch in [-90.0, 90.0] {
135            let got = euler_yxz_deg_from_quat(quat_from_euler_yxz_deg([pitch, 30.0, 40.0]));
136            assert!((got[0] - pitch).abs() < 1e-2, "pitch became {}", got[0]);
137            assert_eq!(got[2], 0.0, "roll should fold into yaw");
138        }
139    }
140
141    #[test]
142    fn a_degenerate_quaternion_normalises_to_the_identity() {
143        assert_eq!(quat_normalize([0.0; 4]), [0.0, 0.0, 0.0, 1.0]);
144        assert_eq!(euler_yxz_deg_from_quat([0.0; 4]), [0.0, 0.0, 0.0]);
145    }
146
147    #[test]
148    fn the_quaternion_built_is_already_unit_length() {
149        for e in [[0.0, 0.0, 0.0], [12.0, -34.0, 56.0], [-89.0, 179.0, 61.0]] {
150            let q = quat_from_euler_yxz_deg(e);
151            let len2 = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
152            assert!((len2 - 1.0).abs() < 1e-5, "{e:?} gave length^2 {len2}");
153        }
154    }
155}
concinnity_core/math/rotation.rs

concinnity_core/math/
rotation.rs
