rakata_formats/mdl/
orientation.rs

1//! Quaternion and axis-angle conversion utilities for MDL orientation data.
2//!
3//! Binary MDL stores orientations as quaternions `[w, x, y, z]`.
4//! ASCII MDL uses axis-angle representation `x y z angle` (angle in radians).
5//! These functions convert between the two representations.
6
7/// Converts a quaternion `[w, x, y, z]` to axis-angle `[ax, ay, az, angle]`.
8///
9/// The angle is in radians. Identity quaternion `[1, 0, 0, 0]` produces
10/// `[0, 0, 0, 0]` (zero rotation).
11#[must_use]
12pub fn quat_to_axis_angle(q: [f32; 4]) -> [f32; 4] {
13    let [w, x, y, z] = q;
14
15    // Normalize the quaternion to handle slight denormalization from f32 math.
16    let len = (w * w + x * x + y * y + z * z).sqrt();
17    if len < f32::EPSILON {
18        return [0.0, 0.0, 0.0, 0.0];
19    }
20    let w = w / len;
21    let x = x / len;
22    let y = y / len;
23    let z = z / len;
24
25    // Ensure w is in [-1, 1] for acos (clamp for floating-point safety).
26    let w_clamped = w.clamp(-1.0, 1.0);
27    let angle = 2.0 * w_clamped.acos();
28
29    let sin_half = (1.0 - w_clamped * w_clamped).sqrt();
30
31    if sin_half < 1.0e-6 {
32        // Near-zero rotation -- axis is arbitrary, return zero vector.
33        [0.0, 0.0, 0.0, 0.0]
34    } else {
35        [x / sin_half, y / sin_half, z / sin_half, angle]
36    }
37}
38
39/// Converts axis-angle `[ax, ay, az, angle]` to quaternion `[w, x, y, z]`.
40///
41/// The angle is in radians. A zero-length axis or zero angle produces the
42/// identity quaternion `[1, 0, 0, 0]`.
43#[must_use]
44pub fn axis_angle_to_quat(aa: [f32; 4]) -> [f32; 4] {
45    let [ax, ay, az, angle] = aa;
46
47    let axis_len = (ax * ax + ay * ay + az * az).sqrt();
48    if axis_len < 1.0e-6 || angle.abs() < 1.0e-6 {
49        return [1.0, 0.0, 0.0, 0.0];
50    }
51
52    // Normalize the axis.
53    let nx = ax / axis_len;
54    let ny = ay / axis_len;
55    let nz = az / axis_len;
56
57    let half = angle * 0.5;
58    let sin_half = half.sin();
59    let cos_half = half.cos();
60
61    [cos_half, nx * sin_half, ny * sin_half, nz * sin_half]
62}
63
64#[cfg(test)]
65mod tests {
66    use super::*;
67
68    fn approx_eq(a: [f32; 4], b: [f32; 4], eps: f32) -> bool {
69        a.iter().zip(b.iter()).all(|(x, y)| (x - y).abs() < eps)
70    }
71
72    #[test]
73    fn identity_quaternion() {
74        let aa = quat_to_axis_angle([1.0, 0.0, 0.0, 0.0]);
75        assert_eq!(aa, [0.0, 0.0, 0.0, 0.0]);
76    }
77
78    #[test]
79    fn identity_axis_angle() {
80        let q = axis_angle_to_quat([0.0, 0.0, 0.0, 0.0]);
81        assert_eq!(q, [1.0, 0.0, 0.0, 0.0]);
82    }
83
84    #[test]
85    fn rotation_90_deg_around_z() {
86        let angle = std::f32::consts::FRAC_PI_2; // 90 degrees
87        let q = axis_angle_to_quat([0.0, 0.0, 1.0, angle]);
88
89        let half = angle * 0.5;
90        let expected = [half.cos(), 0.0, 0.0, half.sin()];
91        assert!(approx_eq(q, expected, 1.0e-6));
92
93        // Roundtrip
94        let aa = quat_to_axis_angle(q);
95        assert!(
96            (aa[3] - angle).abs() < 1.0e-5,
97            "angle mismatch: {} vs {}",
98            aa[3],
99            angle
100        );
101        assert!(
102            (aa[2] - 1.0).abs() < 1.0e-5,
103            "axis Z should be 1.0: {}",
104            aa[2]
105        );
106    }
107
108    #[test]
109    fn rotation_180_deg_around_x() {
110        let angle = std::f32::consts::PI;
111        let q = axis_angle_to_quat([1.0, 0.0, 0.0, angle]);
112
113        let aa = quat_to_axis_angle(q);
114        assert!(
115            (aa[3] - angle).abs() < 1.0e-4,
116            "angle: {} vs {}",
117            aa[3],
118            angle
119        );
120        assert!((aa[0] - 1.0).abs() < 1.0e-4, "axis X: {}", aa[0]);
121    }
122
123    #[test]
124    fn roundtrip_arbitrary_rotation() {
125        let original = [0.5, 0.5, 0.5, 0.5]; // 120 deg around [1,1,1]
126        let aa = quat_to_axis_angle(original);
127        let recovered = axis_angle_to_quat(aa);
128
129        // Quaternions q and -q represent the same rotation.
130        let same = approx_eq(original, recovered, 1.0e-5);
131        let negated = approx_eq(
132            original,
133            [-recovered[0], -recovered[1], -recovered[2], -recovered[3]],
134            1.0e-5,
135        );
136        assert!(
137            same || negated,
138            "roundtrip failed: {original:?} -> {aa:?} -> {recovered:?}"
139        );
140    }
141
142    #[test]
143    fn near_zero_rotation() {
144        // Very small rotation -- should not produce NaN.
145        let q = [0.9999999, 0.0000001, 0.0, 0.0];
146        let aa = quat_to_axis_angle(q);
147        assert!(aa.iter().all(|v| v.is_finite()), "NaN in result: {aa:?}");
148    }
149
150    #[test]
151    fn unnormalized_quaternion() {
152        // Scale of [0.5, 0.5, 0.5, 0.5] * 2
153        let q = [1.0, 1.0, 1.0, 1.0];
154        let aa = quat_to_axis_angle(q);
155        assert!(aa.iter().all(|v| v.is_finite()), "NaN in result: {aa:?}");
156
157        // Should produce same result as normalized version.
158        let aa_norm = quat_to_axis_angle([0.5, 0.5, 0.5, 0.5]);
159        assert!(approx_eq(aa, aa_norm, 1.0e-5));
160    }
161}
rakata_formats/mdl/orientation.rs

rakata_formats/mdl/
orientation.rs
