use std::collections::{HashMap, VecDeque};
use crate::Avatar;
use crate::instance::BoneUpdate;
use crate::node::AvatarNode;
use glam::{Mat4, Quat, Vec3};
use serde::{Deserialize, Serialize};
pub(crate) const VRMC_NODE_CONSTRAINT: &str = "VRMC_node_constraint";
#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
pub enum RollAxis {
X,
Y,
Z,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
pub enum AimAxis {
PositiveX,
NegativeX,
PositiveY,
NegativeY,
PositiveZ,
NegativeZ,
}
#[derive(Debug, Clone, Serialize, Deserialize)]
#[serde(rename_all = "camelCase")]
pub struct RollConstraint {
pub source: u32,
pub roll_axis: RollAxis,
#[serde(default = "default_weight")]
pub weight: f32,
}
#[derive(Debug, Clone, Serialize, Deserialize)]
#[serde(rename_all = "camelCase")]
pub struct AimConstraint {
pub source: u32,
pub aim_axis: AimAxis,
#[serde(default = "default_weight")]
pub weight: f32,
}
#[derive(Debug, Clone, Serialize, Deserialize)]
#[serde(rename_all = "camelCase")]
pub struct RotationConstraint {
pub source: u32,
#[serde(default = "default_weight")]
pub weight: f32,
}
fn default_weight() -> f32 {
1.0
}
#[derive(Debug, Clone, Serialize, Deserialize)]
#[serde(rename_all = "camelCase")]
pub(crate) struct ConstraintDetails {
pub roll: Option<RollConstraint>,
pub aim: Option<AimConstraint>,
pub rotation: Option<RotationConstraint>,
}
#[derive(Debug, Clone, Serialize, Deserialize)]
#[serde(rename_all = "camelCase")]
pub(crate) struct VrmcNodeConstraintSchema {
pub spec_version: String,
pub constraint: ConstraintDetails,
}
#[derive(Debug, Clone)]
pub enum NodeConstraintKind {
Roll(RollConstraint),
Aim(AimConstraint),
Rotation(RotationConstraint),
}
#[derive(Debug, Clone)]
pub struct NodeConstraint {
pub node_index: u32,
pub constraint: NodeConstraintKind,
}
pub(crate) fn load_constraints(
doc: &gltf::Document,
) -> Result<Vec<NodeConstraint>, crate::LoadError> {
let mut constraints = Vec::new();
for node in doc.nodes() {
let Some(value) = node.extension_value(VRMC_NODE_CONSTRAINT) else {
continue;
};
let schema: VrmcNodeConstraintSchema =
serde_json::from_value(value.to_owned()).map_err(|source| {
crate::LoadError::InvalidExtension {
extension: VRMC_NODE_CONSTRAINT,
location: format!("/nodes/{}", node.index()),
source,
}
})?;
if schema.spec_version != "1.0" {
return Err(crate::LoadError::InvalidVrm(format!(
"/nodes/{}/extensions/{VRMC_NODE_CONSTRAINT}/specVersion must be `1.0`",
node.index()
)));
}
let details = schema.constraint;
let variant_count = usize::from(details.roll.is_some())
+ usize::from(details.aim.is_some())
+ usize::from(details.rotation.is_some());
if variant_count != 1 {
return Err(crate::LoadError::InvalidVrm(format!(
"/nodes/{}/extensions/{VRMC_NODE_CONSTRAINT}/constraint must contain exactly one constraint",
node.index()
)));
}
let constraint = details
.roll
.map(NodeConstraintKind::Roll)
.or_else(|| details.aim.map(NodeConstraintKind::Aim))
.or_else(|| details.rotation.map(NodeConstraintKind::Rotation))
.expect("the variant count was checked above");
let item = NodeConstraint {
node_index: node.index() as u32,
constraint,
};
let source = constraint_source(&item).expect("every constraint has a source");
let weight = match &item.constraint {
NodeConstraintKind::Roll(value) => value.weight,
NodeConstraintKind::Aim(value) => value.weight,
NodeConstraintKind::Rotation(value) => value.weight,
};
if source as usize >= doc.nodes().count() || source == item.node_index {
return Err(crate::LoadError::InvalidVrm(format!(
"/nodes/{}/extensions/{VRMC_NODE_CONSTRAINT}/constraint/source is invalid",
node.index()
)));
}
if !weight.is_finite() || !(0.0..=1.0).contains(&weight) {
return Err(crate::LoadError::InvalidVrm(format!(
"/nodes/{}/extensions/{VRMC_NODE_CONSTRAINT}/constraint/weight must be in 0..=1",
node.index()
)));
}
constraints.push(item);
}
if dependency_order(&constraints).len() != constraints.len() {
return Err(crate::LoadError::InvalidVrm(
"VRMC_node_constraint contains a circular dependency".to_string(),
));
}
Ok(constraints)
}
pub fn solve_constraints(avatar: &Avatar, world_matrices: &[Mat4]) -> Vec<BoneUpdate> {
let nodes = avatar.nodes();
let constraints = avatar.node_constraints();
if nodes.is_empty() || constraints.is_empty() {
return Vec::new();
}
let mut world: Vec<Mat4> = world_matrices.to_vec();
let mut local_rotations: Vec<Quat> = nodes
.iter()
.map(|node| {
let idx = node.index as usize;
let local = match node.parent {
Some(p) if (p as usize) < world.len() => world[p as usize].inverse() * world[idx],
_ => world[idx],
};
local.to_scale_rotation_translation().1
})
.collect();
let mut output = Vec::new();
for i in dependency_order(constraints) {
let constraint = &constraints[i];
let Some(delta) = solve_one(nodes, &world, &local_rotations, constraint) else {
continue;
};
if delta.w.abs() > 1.0 - 1e-4 && delta.xyz().length_squared() < 1e-4 {
continue;
}
let idx = constraint.node_index as usize;
local_rotations[idx] = nodes[idx].initial_rotation * delta;
world = compute_world(nodes, &local_rotations);
output.push(BoneUpdate::new(constraint.node_index, delta));
}
output
}
fn solve_one(
nodes: &[AvatarNode],
world: &[Mat4],
local_rotations: &[Quat],
constraint: &NodeConstraint,
) -> Option<Quat> {
let dst = constraint.node_index as usize;
if dst >= nodes.len() {
return None;
}
let dst_rest = nodes[dst].initial_rotation;
match &constraint.constraint {
NodeConstraintKind::Roll(roll) => {
let src = roll.source as usize;
if src >= nodes.len() || src == dst {
return None;
}
let src_rest = nodes[src].initial_rotation;
let src_quat = local_rotations[src];
let delta_src = src_rest.conjugate() * src_quat;
let delta_in_parent = src_rest * delta_src * src_rest.conjugate();
let delta_in_dst = dst_rest.conjugate() * delta_in_parent * dst_rest;
let axis = roll_axis_vec(roll.roll_axis);
let to = delta_in_dst * axis;
let from_to = Quat::from_rotation_arc(axis, to);
let target = dst_rest.slerp(
dst_rest * from_to.conjugate() * delta_in_dst,
roll.weight.clamp(0.0, 1.0),
);
Some(dst_rest.conjugate() * target)
}
NodeConstraintKind::Aim(aim) => {
let src = aim.source as usize;
if src >= nodes.len() || src == dst {
return None;
}
let dst_parent_world = match nodes[dst].parent {
Some(p) if (p as usize) < world.len() => world[p as usize],
_ => Mat4::IDENTITY,
};
let dst_parent_quat = dst_parent_world.to_scale_rotation_translation().1;
let from =
(dst_parent_quat * dst_rest * aim_axis_vec(aim.aim_axis)).normalize_or_zero();
if from == Vec3::ZERO {
return None;
}
let dst_pos = world[dst].transform_point3(Vec3::ZERO);
let src_pos = world[src].transform_point3(Vec3::ZERO);
let to = (src_pos - dst_pos).normalize_or_zero();
if to == Vec3::ZERO {
return None;
}
let from_to = Quat::from_rotation_arc(from, to);
let target = dst_rest.slerp(
dst_parent_quat.conjugate() * from_to * dst_parent_quat * dst_rest,
aim.weight.clamp(0.0, 1.0),
);
Some(dst_rest.conjugate() * target)
}
NodeConstraintKind::Rotation(rotation) => {
let src = rotation.source as usize;
if src >= nodes.len() || src == dst {
return None;
}
let src_rest = nodes[src].initial_rotation;
let src_delta = src_rest.conjugate() * local_rotations[src];
let target = dst_rest.slerp(dst_rest * src_delta, rotation.weight.clamp(0.0, 1.0));
Some(dst_rest.conjugate() * target)
}
}
}
fn dependency_order(constraints: &[NodeConstraint]) -> Vec<usize> {
let mut by_node: HashMap<u32, usize> = HashMap::new();
for (i, constraint) in constraints.iter().enumerate() {
by_node.insert(constraint.node_index, i);
}
let n = constraints.len();
let mut dependents: Vec<Vec<usize>> = vec![Vec::new(); n];
let mut in_degree = vec![0usize; n];
for (j, constraint) in constraints.iter().enumerate() {
let Some(source) = constraint_source(constraint) else {
continue;
};
if source == constraint.node_index {
continue;
}
if let Some(&k) = by_node.get(&source) {
dependents[k].push(j);
in_degree[j] += 1;
}
}
let mut queue: VecDeque<usize> = (0..n).filter(|&i| in_degree[i] == 0).collect();
let mut order = Vec::with_capacity(n);
while let Some(i) = queue.pop_front() {
order.push(i);
for &j in &dependents[i] {
in_degree[j] -= 1;
if in_degree[j] == 0 {
queue.push_back(j);
}
}
}
order
}
pub(crate) fn validate_programmatic(
constraints: &[NodeConstraint],
node_count: usize,
) -> Result<(), String> {
let mut destinations = std::collections::HashSet::new();
for constraint in constraints {
if constraint.node_index as usize >= node_count {
return Err(format!(
"constraint destination node {} is out of range",
constraint.node_index
));
}
if !destinations.insert(constraint.node_index) {
return Err(format!(
"node {} has more than one constraint",
constraint.node_index
));
}
let source = constraint_source(constraint)
.expect("every public constraint variant has a source node");
let weight = match &constraint.constraint {
NodeConstraintKind::Roll(value) => value.weight,
NodeConstraintKind::Aim(value) => value.weight,
NodeConstraintKind::Rotation(value) => value.weight,
};
if source as usize >= node_count || source == constraint.node_index {
return Err(format!(
"constraint on node {} has invalid source node {source}",
constraint.node_index
));
}
if !weight.is_finite() || !(0.0..=1.0).contains(&weight) {
return Err(format!(
"constraint on node {} has a weight outside 0..=1",
constraint.node_index
));
}
}
if dependency_order(constraints).len() != constraints.len() {
return Err("node constraints contain a circular dependency".to_string());
}
Ok(())
}
fn constraint_source(constraint: &NodeConstraint) -> Option<u32> {
match &constraint.constraint {
NodeConstraintKind::Roll(roll) => Some(roll.source),
NodeConstraintKind::Aim(aim) => Some(aim.source),
NodeConstraintKind::Rotation(rotation) => Some(rotation.source),
}
}
fn roll_axis_vec(axis: RollAxis) -> Vec3 {
match axis {
RollAxis::X => Vec3::X,
RollAxis::Y => Vec3::Y,
RollAxis::Z => Vec3::Z,
}
}
fn aim_axis_vec(axis: AimAxis) -> Vec3 {
match axis {
AimAxis::PositiveX => Vec3::X,
AimAxis::NegativeX => Vec3::NEG_X,
AimAxis::PositiveY => Vec3::Y,
AimAxis::NegativeY => Vec3::NEG_Y,
AimAxis::PositiveZ => Vec3::Z,
AimAxis::NegativeZ => Vec3::NEG_Z,
}
}
fn compute_world(nodes: &[AvatarNode], local_rotations: &[Quat]) -> Vec<Mat4> {
let local_matrices = nodes
.iter()
.zip(local_rotations)
.map(|(node, &rotation)| {
Mat4::from_scale_rotation_translation(
node.initial_scale,
rotation,
node.initial_translation,
)
})
.collect::<Vec<_>>();
crate::node::compute_world_from_local_matrices(nodes, &local_matrices)
}
#[cfg(test)]
mod tests {
use super::*;
fn node(index: u32, translation: Vec3, parent: Option<u32>) -> AvatarNode {
AvatarNode {
index,
name: None,
parent,
children: Vec::new(),
initial_translation: translation,
initial_rotation: Quat::IDENTITY,
initial_scale: Vec3::ONE,
}
}
fn world_from(avatar: &Avatar, updates: &[BoneUpdate]) -> Vec<Mat4> {
avatar.compute_world_matrices(updates)
}
fn solved(avatar: &Avatar, world: &[Mat4]) -> Vec<BoneUpdate> {
solve_constraints(avatar, world)
}
#[test]
fn no_constraints_yield_no_updates() {
let avatar = Avatar::new().with_nodes(vec![node(0, Vec3::ZERO, None)]);
let world = world_from(&avatar, &[]);
assert!(solved(&avatar, &world).is_empty());
}
#[test]
fn rotation_constraint_copies_source_local_rotation() {
let avatar =
Avatar::new().with_nodes(vec![node(0, Vec3::ZERO, None), node(1, Vec3::ZERO, None)]);
let avatar = avatar.with_node_constraints(vec![NodeConstraint {
node_index: 1,
constraint: NodeConstraintKind::Rotation(RotationConstraint {
source: 0,
weight: 1.0,
}),
}]);
let source_delta = Quat::from_rotation_z(std::f32::consts::FRAC_PI_2);
let world = world_from(&avatar, &[BoneUpdate::new(0, source_delta)]);
let updates = solved(&avatar, &world);
assert_eq!(updates.len(), 1);
assert_eq!(updates[0].node_index(), 1);
assert!(
updates[0].rotation().angle_between(source_delta) < 5e-3,
"destination should copy the source rotation"
);
}
#[test]
fn rotation_constraint_weight_slerps() {
let avatar =
Avatar::new().with_nodes(vec![node(0, Vec3::ZERO, None), node(1, Vec3::ZERO, None)]);
let avatar = avatar.with_node_constraints(vec![NodeConstraint {
node_index: 1,
constraint: NodeConstraintKind::Rotation(RotationConstraint {
source: 0,
weight: 0.5,
}),
}]);
let source_delta = Quat::from_rotation_z(std::f32::consts::FRAC_PI_2);
let world = world_from(&avatar, &[BoneUpdate::new(0, source_delta)]);
let updates = solved(&avatar, &world);
assert_eq!(updates.len(), 1);
let expected = Quat::from_rotation_z(std::f32::consts::FRAC_PI_4);
assert!(
updates[0].rotation().angle_between(expected) < 5e-3,
"weight 0.5 should produce the halfway rotation"
);
}
#[test]
fn aim_constraint_points_axis_at_source() {
let avatar =
Avatar::new().with_nodes(vec![node(0, Vec3::ZERO, None), node(1, Vec3::Y, None)]);
let avatar = avatar.with_node_constraints(vec![NodeConstraint {
node_index: 0,
constraint: NodeConstraintKind::Aim(AimConstraint {
source: 1,
aim_axis: AimAxis::PositiveZ,
weight: 1.0,
}),
}]);
let world = world_from(&avatar, &[]);
let updates = solved(&avatar, &world);
assert_eq!(updates.len(), 1);
assert_eq!(updates[0].node_index(), 0);
let aimed = updates[0].rotation() * Vec3::Z;
assert!(
aimed.distance(Vec3::Y) < 1e-3,
"the +Z axis should point at the source, got {aimed:?}"
);
}
#[test]
fn roll_constraint_transfers_axis_rotation() {
let avatar =
Avatar::new().with_nodes(vec![node(0, Vec3::ZERO, None), node(1, Vec3::ZERO, None)]);
let avatar = avatar.with_node_constraints(vec![NodeConstraint {
node_index: 1,
constraint: NodeConstraintKind::Roll(RollConstraint {
source: 0,
roll_axis: RollAxis::X,
weight: 1.0,
}),
}]);
let source_delta = Quat::from_rotation_x(std::f32::consts::FRAC_PI_2);
let world = world_from(&avatar, &[BoneUpdate::new(0, source_delta)]);
let updates = solved(&avatar, &world);
assert_eq!(updates.len(), 1);
assert_eq!(updates[0].node_index(), 1);
assert!(
updates[0].rotation().angle_between(source_delta) < 5e-3,
"the roll around X should be copied to the destination"
);
}
#[test]
fn self_referencing_constraint_is_ignored() {
let avatar = Avatar::new().with_nodes(vec![node(0, Vec3::ZERO, None)]);
let avatar = avatar.with_node_constraints(vec![NodeConstraint {
node_index: 0,
constraint: NodeConstraintKind::Rotation(RotationConstraint {
source: 0,
weight: 1.0,
}),
}]);
let source_delta = Quat::from_rotation_z(std::f32::consts::FRAC_PI_2);
let world = world_from(&avatar, &[BoneUpdate::new(0, source_delta)]);
assert!(solved(&avatar, &world).is_empty());
}
#[test]
fn circular_dependencies_are_skipped() {
let avatar = Avatar::new().with_nodes(vec![
node(0, Vec3::ZERO, None),
node(1, Vec3::ZERO, None),
node(2, Vec3::ZERO, None),
]);
let avatar = avatar.with_node_constraints(vec![
NodeConstraint {
node_index: 1,
constraint: NodeConstraintKind::Rotation(RotationConstraint {
source: 2,
weight: 1.0,
}),
},
NodeConstraint {
node_index: 2,
constraint: NodeConstraintKind::Rotation(RotationConstraint {
source: 1,
weight: 1.0,
}),
},
]);
let source_delta = Quat::from_rotation_z(std::f32::consts::FRAC_PI_2);
let world = world_from(&avatar, &[BoneUpdate::new(0, source_delta)]);
assert!(solved(&avatar, &world).is_empty());
}
#[test]
fn dependency_chain_resolves_in_order() {
let avatar = Avatar::new().with_nodes(vec![
node(0, Vec3::ZERO, None),
node(1, Vec3::ZERO, None),
node(2, Vec3::ZERO, None),
]);
let avatar = avatar.with_node_constraints(vec![
NodeConstraint {
node_index: 1,
constraint: NodeConstraintKind::Rotation(RotationConstraint {
source: 0,
weight: 1.0,
}),
},
NodeConstraint {
node_index: 2,
constraint: NodeConstraintKind::Rotation(RotationConstraint {
source: 1,
weight: 1.0,
}),
},
]);
let source_delta = Quat::from_rotation_z(std::f32::consts::FRAC_PI_2);
let world = world_from(&avatar, &[BoneUpdate::new(0, source_delta)]);
let updates = solved(&avatar, &world);
assert_eq!(updates.len(), 2);
for update in &updates {
assert!(
update.rotation().angle_between(source_delta) < 5e-3,
"node {} should copy the source rotation",
update.node_index()
);
}
}
}