1use crate::instance::BoneUpdate;
2use crate::skin::Skin;
3use glam::{Mat4, Quat, Vec3};
4
5#[derive(Debug, Clone)]
8pub struct AvatarNode {
9 pub index: u32,
11 pub name: Option<String>,
13 pub parent: Option<u32>,
15 pub children: Vec<u32>,
17 pub initial_translation: Vec3,
19 pub initial_rotation: Quat,
21 pub initial_scale: Vec3,
23}
24
25impl AvatarNode {
26 pub fn initial_local_matrix(&self) -> Mat4 {
28 Mat4::from_scale_rotation_translation(
29 self.initial_scale,
30 self.initial_rotation,
31 self.initial_translation,
32 )
33 }
34}
35
36pub(crate) fn load_nodes(doc: &gltf::Document) -> Vec<AvatarNode> {
37 let mut parent_map = std::collections::HashMap::new();
38 for node in doc.nodes() {
39 let parent_idx = node.index() as u32;
40 for child in node.children() {
41 parent_map.insert(child.index() as u32, parent_idx);
42 }
43 }
44
45 doc.nodes()
46 .map(|node| {
47 let index = node.index() as u32;
48 let name = node.name().map(|s| s.to_string());
49 let parent = parent_map.get(&index).copied();
50 let children = node.children().map(|c| c.index() as u32).collect();
51
52 let (trans, rot, scale) = node.transform().decomposed();
53
54 AvatarNode {
55 index,
56 name,
57 parent,
58 children,
59 initial_translation: Vec3::from_array(trans),
60 initial_rotation: Quat::from_array(rot),
61 initial_scale: Vec3::from_array(scale),
62 }
63 })
64 .collect()
65}
66
67pub fn compute_world_matrices(nodes: &[AvatarNode], bone_updates: &[BoneUpdate]) -> Vec<Mat4> {
76 let mut local_rotations: Vec<Quat> = nodes.iter().map(|n| n.initial_rotation).collect();
77 let mut local_translations: Vec<Vec3> = nodes.iter().map(|n| n.initial_translation).collect();
78
79 for update in bone_updates {
80 let idx = update.node_index() as usize;
81 if idx < local_rotations.len() {
82 local_rotations[idx] = nodes[idx].initial_rotation * update.rotation();
83 if let Some(translation) = update.translation() {
84 local_translations[idx] = nodes[idx].initial_translation + translation;
85 }
86 }
87 }
88
89 let local_matrices = nodes
90 .iter()
91 .enumerate()
92 .map(|(index, node)| {
93 Mat4::from_scale_rotation_translation(
94 node.initial_scale,
95 local_rotations[index],
96 local_translations[index],
97 )
98 })
99 .collect::<Vec<_>>();
100
101 compute_world_from_local_matrices(nodes, &local_matrices)
102}
103
104pub(crate) fn compute_world_from_local_matrices(
110 nodes: &[AvatarNode],
111 local_matrices: &[Mat4],
112) -> Vec<Mat4> {
113 debug_assert_eq!(nodes.len(), local_matrices.len());
114
115 const UNVISITED: u8 = 0;
116 const VISITING: u8 = 1;
117 const RESOLVED: u8 = 2;
118
119 let mut world = vec![Mat4::IDENTITY; nodes.len()];
120 let mut state = vec![UNVISITED; nodes.len()];
121 let mut chain = Vec::new();
122
123 for start in 0..nodes.len() {
124 if state[start] == RESOLVED {
125 continue;
126 }
127
128 chain.clear();
129 let mut current = start;
130 loop {
131 match state[current] {
132 RESOLVED => break,
133 VISITING => {
134 let cycle_start = chain
139 .iter()
140 .position(|&index| index == current)
141 .expect("a visiting node belongs to the active chain");
142 for &index in &chain[cycle_start..] {
143 world[index] = finite_or_identity(local_matrices[index]);
144 state[index] = RESOLVED;
145 }
146 break;
147 }
148 UNVISITED => {
149 state[current] = VISITING;
150 chain.push(current);
151 match nodes[current].parent.map(|parent| parent as usize) {
152 Some(parent) if parent < nodes.len() => current = parent,
153 _ => break,
154 }
155 }
156 _ => unreachable!("node traversal state is internal"),
157 }
158 }
159
160 for &index in chain.iter().rev() {
161 if state[index] == RESOLVED {
162 continue;
163 }
164 let local = finite_or_identity(local_matrices[index]);
165 let parent_world = nodes[index]
166 .parent
167 .map(|parent| parent as usize)
168 .filter(|&parent| parent < nodes.len() && state[parent] == RESOLVED)
169 .map(|parent| world[parent])
170 .unwrap_or(Mat4::IDENTITY);
171 let composed = parent_world * local;
172 world[index] = if composed.is_finite() {
173 composed
174 } else {
175 local
176 };
177 state[index] = RESOLVED;
178 }
179 }
180
181 world
182}
183
184fn finite_or_identity(matrix: Mat4) -> Mat4 {
185 if matrix.is_finite() {
186 matrix
187 } else {
188 Mat4::IDENTITY
189 }
190}
191
192pub fn compute_joint_matrices(
195 nodes: &[AvatarNode],
196 skin: &Skin,
197 bone_updates: &[BoneUpdate],
198) -> Vec<Mat4> {
199 let world_matrices = compute_world_matrices(nodes, bone_updates);
200
201 skin.joints()
202 .iter()
203 .zip(skin.inverse_bind_matrices().iter())
204 .map(|(&joint_node, &ibm)| {
205 let j_idx = joint_node as usize;
206 let world_mat = if j_idx < world_matrices.len() {
207 world_matrices[j_idx]
208 } else {
209 Mat4::IDENTITY
210 };
211 world_mat * ibm
212 })
213 .collect()
214}
215
216#[cfg(test)]
217mod tests {
218 use super::*;
219
220 fn node(index: u32, parent: Option<u32>, translation: Vec3) -> AvatarNode {
221 AvatarNode {
222 index,
223 name: None,
224 parent,
225 children: Vec::new(),
226 initial_translation: translation,
227 initial_rotation: Quat::IDENTITY,
228 initial_scale: Vec3::ONE,
229 }
230 }
231
232 #[test]
233 fn computes_a_deep_reverse_ordered_hierarchy_without_recursion() {
234 const NODE_COUNT: usize = 50_000;
235 let nodes = (0..NODE_COUNT)
236 .map(|index| {
237 let parent = (index + 1 < NODE_COUNT).then_some((index + 1) as u32);
238 node(index as u32, parent, Vec3::X)
239 })
240 .collect::<Vec<_>>();
241
242 let world = compute_world_matrices(&nodes, &[]);
243
244 assert_eq!(world.len(), NODE_COUNT);
245 assert_eq!(world[0].transform_point3(Vec3::ZERO).x, NODE_COUNT as f32);
246 assert!(world.iter().all(|matrix| matrix.is_finite()));
247 }
248
249 #[test]
250 fn cyclic_raw_nodes_use_a_deterministic_finite_fallback() {
251 let nodes = vec![node(0, Some(1), Vec3::X), node(1, Some(0), Vec3::Y)];
252
253 let first = compute_world_matrices(&nodes, &[]);
254 let second = compute_world_matrices(&nodes, &[]);
255
256 assert_eq!(first, second);
257 assert!(first.iter().all(|matrix| matrix.is_finite()));
258 assert_eq!(first[0], nodes[0].initial_local_matrix());
259 assert_eq!(first[1], nodes[1].initial_local_matrix());
260 }
261}