oxihuman_core/
quaternion_math.rs

1// Copyright (C) 2026 COOLJAPAN OU (Team KitaSan)
2// SPDX-License-Identifier: Apache-2.0
3#![allow(dead_code)]
4
5//! Quaternion math utilities. Quaternion layout: [x, y, z, w].
6
7use std::f32::consts::PI;
8
9/// A quaternion [x, y, z, w].
10#[derive(Debug, Clone, Copy, PartialEq)]
11pub struct QuatMath {
12    pub x: f32,
13    pub y: f32,
14    pub z: f32,
15    pub w: f32,
16}
17
18/// Identity quaternion [0, 0, 0, 1].
19pub fn quat_identity() -> QuatMath {
20    QuatMath {
21        x: 0.0,
22        y: 0.0,
23        z: 0.0,
24        w: 1.0,
25    }
26}
27
28/// Multiply two quaternions.
29pub fn quat_mul(a: QuatMath, b: QuatMath) -> QuatMath {
30    QuatMath {
31        x: a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
32        y: a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
33        z: a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
34        w: a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
35    }
36}
37
38/// Conjugate of a quaternion.
39pub fn quat_conjugate(q: QuatMath) -> QuatMath {
40    QuatMath {
41        x: -q.x,
42        y: -q.y,
43        z: -q.z,
44        w: q.w,
45    }
46}
47
48/// Squared norm of a quaternion.
49pub fn quat_norm_sq(q: QuatMath) -> f32 {
50    q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w
51}
52
53/// Normalize a quaternion to unit length.
54pub fn quat_normalize(q: QuatMath) -> QuatMath {
55    let len = quat_norm_sq(q).sqrt();
56    if len < 1e-12 {
57        return quat_identity();
58    }
59    QuatMath {
60        x: q.x / len,
61        y: q.y / len,
62        z: q.z / len,
63        w: q.w / len,
64    }
65}
66
67/// Build a rotation quaternion from axis (normalized) and angle in radians.
68pub fn quat_from_axis_angle(axis: [f32; 3], angle_rad: f32) -> QuatMath {
69    let half = angle_rad * 0.5;
70    let s = half.sin();
71    QuatMath {
72        x: axis[0] * s,
73        y: axis[1] * s,
74        z: axis[2] * s,
75        w: half.cos(),
76    }
77}
78
79/// Rotate a 3D vector by a quaternion (q must be normalized).
80pub fn quat_rotate_vec3(q: QuatMath, v: [f32; 3]) -> [f32; 3] {
81    let vq = QuatMath {
82        x: v[0],
83        y: v[1],
84        z: v[2],
85        w: 0.0,
86    };
87    let res = quat_mul(quat_mul(q, vq), quat_conjugate(q));
88    [res.x, res.y, res.z]
89}
90
91/// Spherical linear interpolation between two quaternions.
92pub fn quat_slerp(a: QuatMath, b: QuatMath, t: f32) -> QuatMath {
93    let mut dot = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
94    let b = if dot < 0.0 {
95        dot = -dot;
96        QuatMath {
97            x: -b.x,
98            y: -b.y,
99            z: -b.z,
100            w: -b.w,
101        }
102    } else {
103        b
104    };
105    if dot > 0.9995 {
106        return quat_normalize(QuatMath {
107            x: a.x + t * (b.x - a.x),
108            y: a.y + t * (b.y - a.y),
109            z: a.z + t * (b.z - a.z),
110            w: a.w + t * (b.w - a.w),
111        });
112    }
113    let theta_0 = dot.acos();
114    let theta = theta_0 * t;
115    let sin_theta = theta.sin();
116    let sin_theta_0 = theta_0.sin();
117    let s0 = (theta_0 - theta).sin() / sin_theta_0;
118    let s1 = sin_theta / sin_theta_0;
119    QuatMath {
120        x: s0 * a.x + s1 * b.x,
121        y: s0 * a.y + s1 * b.y,
122        z: s0 * a.z + s1 * b.z,
123        w: s0 * a.w + s1 * b.w,
124    }
125}
126
127/// Convert quaternion to Euler angles [roll, pitch, yaw] in radians.
128pub fn quat_to_euler(q: QuatMath) -> [f32; 3] {
129    let sinr_cosp = 2.0 * (q.w * q.x + q.y * q.z);
130    let cosr_cosp = 1.0 - 2.0 * (q.x * q.x + q.y * q.y);
131    let roll = sinr_cosp.atan2(cosr_cosp);
132    let sinp = 2.0 * (q.w * q.y - q.z * q.x);
133    let pitch = if sinp.abs() >= 1.0 {
134        sinp.signum() * PI / 2.0
135    } else {
136        sinp.asin()
137    };
138    let siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
139    let cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
140    let yaw = siny_cosp.atan2(cosy_cosp);
141    [roll, pitch, yaw]
142}
143
144#[cfg(test)]
145mod tests {
146    use super::*;
147    use std::f32::consts::FRAC_PI_2;
148
149    const EPS: f32 = 1e-5;
150
151    #[test]
152    fn test_identity_mul() {
153        /* identity * identity = identity */
154        let id = quat_identity();
155        let r = quat_mul(id, id);
156        assert!((r.w - 1.0).abs() < EPS);
157        assert!(r.x.abs() < EPS);
158    }
159
160    #[test]
161    fn test_normalize_identity() {
162        /* identity is already normalized */
163        let n = quat_normalize(quat_identity());
164        assert!((n.w - 1.0).abs() < EPS);
165    }
166
167    #[test]
168    fn test_conjugate_reverses_rotation() {
169        /* q * conj(q) = identity */
170        let q = quat_from_axis_angle([0.0, 0.0, 1.0], FRAC_PI_2);
171        let r = quat_mul(q, quat_conjugate(q));
172        assert!((r.w - 1.0).abs() < EPS);
173        assert!(r.x.abs() < EPS);
174        assert!(r.y.abs() < EPS);
175        assert!(r.z.abs() < EPS);
176    }
177
178    #[test]
179    fn test_from_axis_angle_z90() {
180        /* 90 degrees around Z rotates [1,0,0] to ~[0,1,0] */
181        let q = quat_from_axis_angle([0.0, 0.0, 1.0], FRAC_PI_2);
182        let v = quat_rotate_vec3(q, [1.0, 0.0, 0.0]);
183        assert!(v[0].abs() < 1e-4);
184        assert!((v[1] - 1.0).abs() < 1e-4);
185    }
186
187    #[test]
188    fn test_slerp_endpoints() {
189        /* slerp at 0 = a, at 1 = b */
190        let a = quat_identity();
191        let b = quat_from_axis_angle([0.0, 1.0, 0.0], FRAC_PI_2);
192        let r0 = quat_slerp(a, b, 0.0);
193        assert!((r0.w - a.w).abs() < EPS);
194        let r1 = quat_slerp(a, b, 1.0);
195        assert!((r1.w - b.w).abs() < EPS);
196    }
197
198    #[test]
199    fn test_slerp_midpoint_norm() {
200        /* slerp midpoint should be normalized */
201        let a = quat_identity();
202        let b = quat_from_axis_angle([1.0, 0.0, 0.0], FRAC_PI_2);
203        let m = quat_slerp(a, b, 0.5);
204        let ns = quat_norm_sq(m).sqrt();
205        assert!((ns - 1.0).abs() < EPS);
206    }
207
208    #[test]
209    fn test_to_euler_zero() {
210        /* identity quaternion -> zero euler */
211        let e = quat_to_euler(quat_identity());
212        assert!(e[0].abs() < EPS);
213        assert!(e[1].abs() < EPS);
214        assert!(e[2].abs() < EPS);
215    }
216
217    #[test]
218    fn test_norm_sq_identity() {
219        /* identity norm^2 = 1 */
220        let ns = quat_norm_sq(quat_identity());
221        assert!((ns - 1.0).abs() < EPS);
222    }
223
224    #[test]
225    fn test_rotate_identity_no_change() {
226        /* rotating by identity quaternion leaves vector unchanged */
227        let q = quat_identity();
228        let v = quat_rotate_vec3(q, [3.0, 4.0, 5.0]);
229        assert!((v[0] - 3.0).abs() < EPS);
230        assert!((v[1] - 4.0).abs() < EPS);
231        assert!((v[2] - 5.0).abs() < EPS);
232    }
233}
oxihuman_core/quaternion_math.rs

oxihuman_core/
quaternion_math.rs
