oxihuman_core/
transform3d.rs

1#![allow(dead_code)]
2// Copyright (C) 2026 COOLJAPAN OU (Team KitaSan)
3// SPDX-License-Identifier: Apache-2.0
4
5//! 3D transform (position + rotation quaternion + scale).
6
7#[allow(dead_code)]
8#[derive(Debug, Clone)]
9pub struct Transform3d {
10    pub position: [f32; 3],
11    /// Quaternion [x, y, z, w]
12    pub rotation: [f32; 4],
13    pub scale: [f32; 3],
14}
15
16#[allow(dead_code)]
17pub fn transform_identity() -> Transform3d {
18    Transform3d {
19        position: [0.0, 0.0, 0.0],
20        rotation: [0.0, 0.0, 0.0, 1.0],
21        scale: [1.0, 1.0, 1.0],
22    }
23}
24
25#[allow(dead_code)]
26pub fn transform_translate(t: &Transform3d, delta: [f32; 3]) -> Transform3d {
27    Transform3d {
28        position: [
29            t.position[0] + delta[0],
30            t.position[1] + delta[1],
31            t.position[2] + delta[2],
32        ],
33        rotation: t.rotation,
34        scale: t.scale,
35    }
36}
37
38/// Multiply two quaternions q1 * q2.
39fn quat_mul_local(a: [f32; 4], b: [f32; 4]) -> [f32; 4] {
40    [
41        a[3] * b[0] + a[0] * b[3] + a[1] * b[2] - a[2] * b[1],
42        a[3] * b[1] - a[0] * b[2] + a[1] * b[3] + a[2] * b[0],
43        a[3] * b[2] + a[0] * b[1] - a[1] * b[0] + a[2] * b[3],
44        a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2],
45    ]
46}
47
48#[allow(dead_code)]
49pub fn transform_rotate(t: &Transform3d, q: [f32; 4]) -> Transform3d {
50    Transform3d {
51        position: t.position,
52        rotation: quat_mul_local(t.rotation, q),
53        scale: t.scale,
54    }
55}
56
57#[allow(dead_code)]
58pub fn transform_scale_uniform(t: &Transform3d, s: f32) -> Transform3d {
59    Transform3d {
60        position: t.position,
61        rotation: t.rotation,
62        scale: [t.scale[0] * s, t.scale[1] * s, t.scale[2] * s],
63    }
64}
65
66#[allow(dead_code)]
67pub fn transform_to_mat4(t: &Transform3d) -> [[f32; 4]; 4] {
68    let q = t.rotation;
69    let qx = q[0];
70    let qy = q[1];
71    let qz = q[2];
72    let qw = q[3];
73    let sx = t.scale[0];
74    let sy = t.scale[1];
75    let sz = t.scale[2];
76    let tx = t.position[0];
77    let ty = t.position[1];
78    let tz = t.position[2];
79
80    [
81        [
82            (1.0 - 2.0 * (qy * qy + qz * qz)) * sx,
83            (2.0 * (qx * qy - qw * qz)) * sy,
84            (2.0 * (qx * qz + qw * qy)) * sz,
85            tx,
86        ],
87        [
88            (2.0 * (qx * qy + qw * qz)) * sx,
89            (1.0 - 2.0 * (qx * qx + qz * qz)) * sy,
90            (2.0 * (qy * qz - qw * qx)) * sz,
91            ty,
92        ],
93        [
94            (2.0 * (qx * qz - qw * qy)) * sx,
95            (2.0 * (qy * qz + qw * qx)) * sy,
96            (1.0 - 2.0 * (qx * qx + qy * qy)) * sz,
97            tz,
98        ],
99        [0.0, 0.0, 0.0, 1.0],
100    ]
101}
102
103fn lerp_f32(a: f32, b: f32, t: f32) -> f32 {
104    a + (b - a) * t
105}
106
107#[allow(dead_code)]
108pub fn transform_lerp(a: &Transform3d, b: &Transform3d, fac: f32) -> Transform3d {
109    let fac = fac.clamp(0.0, 1.0);
110    Transform3d {
111        position: [
112            lerp_f32(a.position[0], b.position[0], fac),
113            lerp_f32(a.position[1], b.position[1], fac),
114            lerp_f32(a.position[2], b.position[2], fac),
115        ],
116        rotation: [
117            lerp_f32(a.rotation[0], b.rotation[0], fac),
118            lerp_f32(a.rotation[1], b.rotation[1], fac),
119            lerp_f32(a.rotation[2], b.rotation[2], fac),
120            lerp_f32(a.rotation[3], b.rotation[3], fac),
121        ],
122        scale: [
123            lerp_f32(a.scale[0], b.scale[0], fac),
124            lerp_f32(a.scale[1], b.scale[1], fac),
125            lerp_f32(a.scale[2], b.scale[2], fac),
126        ],
127    }
128}
129
130/// Apply the transform to a 3D point (position only, no rotation/scale applied here).
131#[allow(dead_code)]
132pub fn transform_apply(t: &Transform3d, p: [f32; 3]) -> [f32; 3] {
133    [
134        t.position[0] + p[0] * t.scale[0],
135        t.position[1] + p[1] * t.scale[1],
136        t.position[2] + p[2] * t.scale[2],
137    ]
138}
139
140/// Combine two transforms: returns a transform that first applies `a` then `b`.
141#[allow(dead_code)]
142pub fn transform_combine(a: &Transform3d, b: &Transform3d) -> Transform3d {
143    Transform3d {
144        position: [
145            a.position[0] + b.position[0],
146            a.position[1] + b.position[1],
147            a.position[2] + b.position[2],
148        ],
149        rotation: quat_mul_local(a.rotation, b.rotation),
150        scale: [
151            a.scale[0] * b.scale[0],
152            a.scale[1] * b.scale[1],
153            a.scale[2] * b.scale[2],
154        ],
155    }
156}
157
158/// Returns the translation inverse (negated position, identity rotation, identity scale).
159#[allow(dead_code)]
160pub fn transform_inverse_translation(t: &Transform3d) -> Transform3d {
161    Transform3d {
162        position: [-t.position[0], -t.position[1], -t.position[2]],
163        rotation: t.rotation,
164        scale: t.scale,
165    }
166}
167
168#[cfg(test)]
169mod tests {
170    use super::*;
171
172    #[test]
173    fn test_identity_position() {
174        let t = transform_identity();
175        assert_eq!(t.position, [0.0, 0.0, 0.0]);
176    }
177
178    #[test]
179    fn test_identity_scale() {
180        let t = transform_identity();
181        assert_eq!(t.scale, [1.0, 1.0, 1.0]);
182    }
183
184    #[test]
185    fn test_translate() {
186        let t = transform_identity();
187        let t2 = transform_translate(&t, [1.0, 2.0, 3.0]);
188        assert!((t2.position[0] - 1.0).abs() < 1e-6);
189        assert!((t2.position[1] - 2.0).abs() < 1e-6);
190        assert!((t2.position[2] - 3.0).abs() < 1e-6);
191    }
192
193    #[test]
194    fn test_scale_uniform() {
195        let t = transform_identity();
196        let t2 = transform_scale_uniform(&t, 2.0);
197        assert!((t2.scale[0] - 2.0).abs() < 1e-6);
198        assert!((t2.scale[1] - 2.0).abs() < 1e-6);
199    }
200
201    #[test]
202    fn test_to_mat4_identity() {
203        let t = transform_identity();
204        let m = transform_to_mat4(&t);
205        assert!((m[0][0] - 1.0).abs() < 1e-6);
206        assert!((m[1][1] - 1.0).abs() < 1e-6);
207        assert!((m[2][2] - 1.0).abs() < 1e-6);
208        assert!((m[3][3] - 1.0).abs() < 1e-6);
209    }
210
211    #[test]
212    fn test_to_mat4_translation() {
213        let t = transform_translate(&transform_identity(), [5.0, 6.0, 7.0]);
214        let m = transform_to_mat4(&t);
215        assert!((m[0][3] - 5.0).abs() < 1e-6);
216        assert!((m[1][3] - 6.0).abs() < 1e-6);
217        assert!((m[2][3] - 7.0).abs() < 1e-6);
218    }
219
220    #[test]
221    fn test_lerp_half() {
222        let a = transform_identity();
223        let b = transform_translate(&transform_identity(), [2.0, 0.0, 0.0]);
224        let c = transform_lerp(&a, &b, 0.5);
225        assert!((c.position[0] - 1.0).abs() < 1e-6);
226    }
227
228    #[test]
229    fn test_lerp_zero() {
230        let a = transform_identity();
231        let b = transform_translate(&transform_identity(), [2.0, 0.0, 0.0]);
232        let c = transform_lerp(&a, &b, 0.0);
233        assert!(c.position[0].abs() < 1e-6);
234    }
235
236    #[test]
237    fn test_lerp_one() {
238        let a = transform_identity();
239        let b = transform_translate(&transform_identity(), [2.0, 0.0, 0.0]);
240        let c = transform_lerp(&a, &b, 1.0);
241        assert!((c.position[0] - 2.0).abs() < 1e-6);
242    }
243
244    #[test]
245    fn test_rotate_identity_unchanged() {
246        let t = transform_identity();
247        let q_id = [0.0f32, 0.0, 0.0, 1.0];
248        let t2 = transform_rotate(&t, q_id);
249        assert!((t2.rotation[3] - 1.0).abs() < 1e-6);
250    }
251}
oxihuman_core/transform3d.rs

oxihuman_core/
transform3d.rs
