use crate::instance::BoneUpdate;
use crate::skin::Skin;
use glam::{Mat4, Quat, Vec3};
#[derive(Debug, Clone)]
pub struct AvatarNode {
pub index: u32,
pub name: Option<String>,
pub parent: Option<u32>,
pub children: Vec<u32>,
pub initial_translation: Vec3,
pub initial_rotation: Quat,
pub initial_scale: Vec3,
}
impl AvatarNode {
pub fn initial_local_matrix(&self) -> Mat4 {
Mat4::from_scale_rotation_translation(
self.initial_scale,
self.initial_rotation,
self.initial_translation,
)
}
}
pub(crate) fn load_nodes(doc: &gltf::Document) -> Vec<AvatarNode> {
let mut parent_map = std::collections::HashMap::new();
for node in doc.nodes() {
let parent_idx = node.index() as u32;
for child in node.children() {
parent_map.insert(child.index() as u32, parent_idx);
}
}
doc.nodes()
.map(|node| {
let index = node.index() as u32;
let name = node.name().map(|s| s.to_string());
let parent = parent_map.get(&index).copied();
let children = node.children().map(|c| c.index() as u32).collect();
let (trans, rot, scale) = node.transform().decomposed();
AvatarNode {
index,
name,
parent,
children,
initial_translation: Vec3::from_array(trans),
initial_rotation: Quat::from_array(rot),
initial_scale: Vec3::from_array(scale),
}
})
.collect()
}
pub fn compute_world_matrices(nodes: &[AvatarNode], bone_updates: &[BoneUpdate]) -> Vec<Mat4> {
let mut local_rotations: Vec<Quat> = nodes.iter().map(|n| n.initial_rotation).collect();
let mut local_translations: Vec<Vec3> = nodes.iter().map(|n| n.initial_translation).collect();
for update in bone_updates {
let idx = update.node_index() as usize;
if idx < local_rotations.len() {
local_rotations[idx] = nodes[idx].initial_rotation * update.rotation();
if let Some(translation) = update.translation() {
local_translations[idx] = nodes[idx].initial_translation + translation;
}
}
}
let local_matrices = nodes
.iter()
.enumerate()
.map(|(index, node)| {
Mat4::from_scale_rotation_translation(
node.initial_scale,
local_rotations[index],
local_translations[index],
)
})
.collect::<Vec<_>>();
compute_world_from_local_matrices(nodes, &local_matrices)
}
pub(crate) fn compute_world_from_local_matrices(
nodes: &[AvatarNode],
local_matrices: &[Mat4],
) -> Vec<Mat4> {
debug_assert_eq!(nodes.len(), local_matrices.len());
const UNVISITED: u8 = 0;
const VISITING: u8 = 1;
const RESOLVED: u8 = 2;
let mut world = vec![Mat4::IDENTITY; nodes.len()];
let mut state = vec![UNVISITED; nodes.len()];
let mut chain = Vec::new();
for start in 0..nodes.len() {
if state[start] == RESOLVED {
continue;
}
chain.clear();
let mut current = start;
loop {
match state[current] {
RESOLVED => break,
VISITING => {
let cycle_start = chain
.iter()
.position(|&index| index == current)
.expect("a visiting node belongs to the active chain");
for &index in &chain[cycle_start..] {
world[index] = finite_or_identity(local_matrices[index]);
state[index] = RESOLVED;
}
break;
}
UNVISITED => {
state[current] = VISITING;
chain.push(current);
match nodes[current].parent.map(|parent| parent as usize) {
Some(parent) if parent < nodes.len() => current = parent,
_ => break,
}
}
_ => unreachable!("node traversal state is internal"),
}
}
for &index in chain.iter().rev() {
if state[index] == RESOLVED {
continue;
}
let local = finite_or_identity(local_matrices[index]);
let parent_world = nodes[index]
.parent
.map(|parent| parent as usize)
.filter(|&parent| parent < nodes.len() && state[parent] == RESOLVED)
.map(|parent| world[parent])
.unwrap_or(Mat4::IDENTITY);
let composed = parent_world * local;
world[index] = if composed.is_finite() {
composed
} else {
local
};
state[index] = RESOLVED;
}
}
world
}
fn finite_or_identity(matrix: Mat4) -> Mat4 {
if matrix.is_finite() {
matrix
} else {
Mat4::IDENTITY
}
}
pub fn compute_joint_matrices(
nodes: &[AvatarNode],
skin: &Skin,
bone_updates: &[BoneUpdate],
) -> Vec<Mat4> {
let world_matrices = compute_world_matrices(nodes, bone_updates);
skin.joints()
.iter()
.zip(skin.inverse_bind_matrices().iter())
.map(|(&joint_node, &ibm)| {
let j_idx = joint_node as usize;
let world_mat = if j_idx < world_matrices.len() {
world_matrices[j_idx]
} else {
Mat4::IDENTITY
};
world_mat * ibm
})
.collect()
}
#[cfg(test)]
mod tests {
use super::*;
fn node(index: u32, parent: Option<u32>, translation: Vec3) -> AvatarNode {
AvatarNode {
index,
name: None,
parent,
children: Vec::new(),
initial_translation: translation,
initial_rotation: Quat::IDENTITY,
initial_scale: Vec3::ONE,
}
}
#[test]
fn computes_a_deep_reverse_ordered_hierarchy_without_recursion() {
const NODE_COUNT: usize = 50_000;
let nodes = (0..NODE_COUNT)
.map(|index| {
let parent = (index + 1 < NODE_COUNT).then_some((index + 1) as u32);
node(index as u32, parent, Vec3::X)
})
.collect::<Vec<_>>();
let world = compute_world_matrices(&nodes, &[]);
assert_eq!(world.len(), NODE_COUNT);
assert_eq!(world[0].transform_point3(Vec3::ZERO).x, NODE_COUNT as f32);
assert!(world.iter().all(|matrix| matrix.is_finite()));
}
#[test]
fn cyclic_raw_nodes_use_a_deterministic_finite_fallback() {
let nodes = vec![node(0, Some(1), Vec3::X), node(1, Some(0), Vec3::Y)];
let first = compute_world_matrices(&nodes, &[]);
let second = compute_world_matrices(&nodes, &[]);
assert_eq!(first, second);
assert!(first.iter().all(|matrix| matrix.is_finite()));
assert_eq!(first[0], nodes[0].initial_local_matrix());
assert_eq!(first[1], nodes[1].initial_local_matrix());
}
}