1use std::collections::{HashMap, VecDeque};
2
3use crate::Avatar;
4use crate::instance::BoneUpdate;
5use crate::node::AvatarNode;
6use glam::{Mat4, Quat, Vec3};
7use serde::{Deserialize, Serialize};
8
9pub(crate) const VRMC_NODE_CONSTRAINT: &str = "VRMC_node_constraint";
10
11#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
13pub enum RollAxis {
14 X,
16 Y,
18 Z,
20}
21
22#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
24pub enum AimAxis {
25 PositiveX,
27 NegativeX,
29 PositiveY,
31 NegativeY,
33 PositiveZ,
35 NegativeZ,
37}
38
39#[derive(Debug, Clone, Serialize, Deserialize)]
42#[serde(rename_all = "camelCase")]
43pub struct RollConstraint {
44 pub source: u32,
46 pub roll_axis: RollAxis,
48 #[serde(default = "default_weight")]
50 pub weight: f32,
51}
52
53#[derive(Debug, Clone, Serialize, Deserialize)]
56#[serde(rename_all = "camelCase")]
57pub struct AimConstraint {
58 pub source: u32,
60 pub aim_axis: AimAxis,
62 #[serde(default = "default_weight")]
64 pub weight: f32,
65}
66
67#[derive(Debug, Clone, Serialize, Deserialize)]
70#[serde(rename_all = "camelCase")]
71pub struct RotationConstraint {
72 pub source: u32,
74 #[serde(default = "default_weight")]
76 pub weight: f32,
77}
78
79fn default_weight() -> f32 {
80 1.0
81}
82
83#[derive(Debug, Clone, Serialize, Deserialize)]
84#[serde(rename_all = "camelCase")]
85pub(crate) struct ConstraintDetails {
86 pub roll: Option<RollConstraint>,
87 pub aim: Option<AimConstraint>,
88 pub rotation: Option<RotationConstraint>,
89}
90
91#[derive(Debug, Clone, Serialize, Deserialize)]
92#[serde(rename_all = "camelCase")]
93pub(crate) struct VrmcNodeConstraintSchema {
94 pub spec_version: String,
95 pub constraint: ConstraintDetails,
96}
97
98#[derive(Debug, Clone)]
100pub enum NodeConstraintKind {
101 Roll(RollConstraint),
103 Aim(AimConstraint),
105 Rotation(RotationConstraint),
107}
108
109#[derive(Debug, Clone)]
111pub struct NodeConstraint {
112 pub node_index: u32,
114 pub constraint: NodeConstraintKind,
116}
117
118pub(crate) fn load_constraints(
119 doc: &gltf::Document,
120) -> Result<Vec<NodeConstraint>, crate::LoadError> {
121 let mut constraints = Vec::new();
122 for node in doc.nodes() {
123 let Some(value) = node.extension_value(VRMC_NODE_CONSTRAINT) else {
124 continue;
125 };
126 let schema: VrmcNodeConstraintSchema =
127 serde_json::from_value(value.to_owned()).map_err(|source| {
128 crate::LoadError::InvalidExtension {
129 extension: VRMC_NODE_CONSTRAINT,
130 location: format!("/nodes/{}", node.index()),
131 source,
132 }
133 })?;
134 if schema.spec_version != "1.0" {
135 return Err(crate::LoadError::InvalidVrm(format!(
136 "/nodes/{}/extensions/{VRMC_NODE_CONSTRAINT}/specVersion must be `1.0`",
137 node.index()
138 )));
139 }
140
141 let details = schema.constraint;
142 let variant_count = usize::from(details.roll.is_some())
143 + usize::from(details.aim.is_some())
144 + usize::from(details.rotation.is_some());
145 if variant_count != 1 {
146 return Err(crate::LoadError::InvalidVrm(format!(
147 "/nodes/{}/extensions/{VRMC_NODE_CONSTRAINT}/constraint must contain exactly one constraint",
148 node.index()
149 )));
150 }
151 let constraint = details
152 .roll
153 .map(NodeConstraintKind::Roll)
154 .or_else(|| details.aim.map(NodeConstraintKind::Aim))
155 .or_else(|| details.rotation.map(NodeConstraintKind::Rotation))
156 .expect("the variant count was checked above");
157 let item = NodeConstraint {
158 node_index: node.index() as u32,
159 constraint,
160 };
161 let source = constraint_source(&item).expect("every constraint has a source");
162 let weight = match &item.constraint {
163 NodeConstraintKind::Roll(value) => value.weight,
164 NodeConstraintKind::Aim(value) => value.weight,
165 NodeConstraintKind::Rotation(value) => value.weight,
166 };
167 if source as usize >= doc.nodes().count() || source == item.node_index {
168 return Err(crate::LoadError::InvalidVrm(format!(
169 "/nodes/{}/extensions/{VRMC_NODE_CONSTRAINT}/constraint/source is invalid",
170 node.index()
171 )));
172 }
173 if !weight.is_finite() || !(0.0..=1.0).contains(&weight) {
174 return Err(crate::LoadError::InvalidVrm(format!(
175 "/nodes/{}/extensions/{VRMC_NODE_CONSTRAINT}/constraint/weight must be in 0..=1",
176 node.index()
177 )));
178 }
179 constraints.push(item);
180 }
181
182 if dependency_order(&constraints).len() != constraints.len() {
183 return Err(crate::LoadError::InvalidVrm(
184 "VRMC_node_constraint contains a circular dependency".to_string(),
185 ));
186 }
187 Ok(constraints)
188}
189
190pub fn solve_constraints(avatar: &Avatar, world_matrices: &[Mat4]) -> Vec<BoneUpdate> {
198 let nodes = avatar.nodes();
199 let constraints = avatar.node_constraints();
200 if nodes.is_empty() || constraints.is_empty() {
201 return Vec::new();
202 }
203
204 let mut world: Vec<Mat4> = world_matrices.to_vec();
206
207 let mut local_rotations: Vec<Quat> = nodes
209 .iter()
210 .map(|node| {
211 let idx = node.index as usize;
212 let local = match node.parent {
213 Some(p) if (p as usize) < world.len() => world[p as usize].inverse() * world[idx],
214 _ => world[idx],
215 };
216 local.to_scale_rotation_translation().1
217 })
218 .collect();
219
220 let mut output = Vec::new();
221
222 for i in dependency_order(constraints) {
223 let constraint = &constraints[i];
224 let Some(delta) = solve_one(nodes, &world, &local_rotations, constraint) else {
225 continue;
226 };
227
228 if delta.w.abs() > 1.0 - 1e-4 && delta.xyz().length_squared() < 1e-4 {
231 continue;
232 }
233
234 let idx = constraint.node_index as usize;
235 local_rotations[idx] = nodes[idx].initial_rotation * delta;
236 world = compute_world(nodes, &local_rotations);
237 output.push(BoneUpdate::new(constraint.node_index, delta));
238 }
239
240 output
241}
242
243fn solve_one(
246 nodes: &[AvatarNode],
247 world: &[Mat4],
248 local_rotations: &[Quat],
249 constraint: &NodeConstraint,
250) -> Option<Quat> {
251 let dst = constraint.node_index as usize;
252 if dst >= nodes.len() {
253 return None;
254 }
255 let dst_rest = nodes[dst].initial_rotation;
256
257 match &constraint.constraint {
258 NodeConstraintKind::Roll(roll) => {
259 let src = roll.source as usize;
260 if src >= nodes.len() || src == dst {
261 return None;
262 }
263
264 let src_rest = nodes[src].initial_rotation;
268 let src_quat = local_rotations[src];
269 let delta_src = src_rest.conjugate() * src_quat;
270 let delta_in_parent = src_rest * delta_src * src_rest.conjugate();
271 let delta_in_dst = dst_rest.conjugate() * delta_in_parent * dst_rest;
272
273 let axis = roll_axis_vec(roll.roll_axis);
274 let to = delta_in_dst * axis;
275 let from_to = Quat::from_rotation_arc(axis, to);
276
277 let target = dst_rest.slerp(
278 dst_rest * from_to.conjugate() * delta_in_dst,
279 roll.weight.clamp(0.0, 1.0),
280 );
281 Some(dst_rest.conjugate() * target)
282 }
283
284 NodeConstraintKind::Aim(aim) => {
285 let src = aim.source as usize;
286 if src >= nodes.len() || src == dst {
287 return None;
288 }
289
290 let dst_parent_world = match nodes[dst].parent {
293 Some(p) if (p as usize) < world.len() => world[p as usize],
294 _ => Mat4::IDENTITY,
295 };
296 let dst_parent_quat = dst_parent_world.to_scale_rotation_translation().1;
297
298 let from =
299 (dst_parent_quat * dst_rest * aim_axis_vec(aim.aim_axis)).normalize_or_zero();
300 if from == Vec3::ZERO {
301 return None;
302 }
303 let dst_pos = world[dst].transform_point3(Vec3::ZERO);
304 let src_pos = world[src].transform_point3(Vec3::ZERO);
305 let to = (src_pos - dst_pos).normalize_or_zero();
306 if to == Vec3::ZERO {
307 return None;
308 }
309
310 let from_to = Quat::from_rotation_arc(from, to);
311 let target = dst_rest.slerp(
312 dst_parent_quat.conjugate() * from_to * dst_parent_quat * dst_rest,
313 aim.weight.clamp(0.0, 1.0),
314 );
315 Some(dst_rest.conjugate() * target)
316 }
317
318 NodeConstraintKind::Rotation(rotation) => {
319 let src = rotation.source as usize;
320 if src >= nodes.len() || src == dst {
321 return None;
322 }
323
324 let src_rest = nodes[src].initial_rotation;
327 let src_delta = src_rest.conjugate() * local_rotations[src];
328
329 let target = dst_rest.slerp(dst_rest * src_delta, rotation.weight.clamp(0.0, 1.0));
330 Some(dst_rest.conjugate() * target)
331 }
332 }
333}
334
335fn dependency_order(constraints: &[NodeConstraint]) -> Vec<usize> {
339 let mut by_node: HashMap<u32, usize> = HashMap::new();
340 for (i, constraint) in constraints.iter().enumerate() {
341 by_node.insert(constraint.node_index, i);
342 }
343
344 let n = constraints.len();
345 let mut dependents: Vec<Vec<usize>> = vec![Vec::new(); n];
346 let mut in_degree = vec![0usize; n];
347
348 for (j, constraint) in constraints.iter().enumerate() {
349 let Some(source) = constraint_source(constraint) else {
350 continue;
351 };
352 if source == constraint.node_index {
353 continue;
354 }
355 if let Some(&k) = by_node.get(&source) {
356 dependents[k].push(j);
357 in_degree[j] += 1;
358 }
359 }
360
361 let mut queue: VecDeque<usize> = (0..n).filter(|&i| in_degree[i] == 0).collect();
362 let mut order = Vec::with_capacity(n);
363 while let Some(i) = queue.pop_front() {
364 order.push(i);
365 for &j in &dependents[i] {
366 in_degree[j] -= 1;
367 if in_degree[j] == 0 {
368 queue.push_back(j);
369 }
370 }
371 }
372 order
373}
374
375pub(crate) fn validate_programmatic(
376 constraints: &[NodeConstraint],
377 node_count: usize,
378) -> Result<(), String> {
379 let mut destinations = std::collections::HashSet::new();
380 for constraint in constraints {
381 if constraint.node_index as usize >= node_count {
382 return Err(format!(
383 "constraint destination node {} is out of range",
384 constraint.node_index
385 ));
386 }
387 if !destinations.insert(constraint.node_index) {
388 return Err(format!(
389 "node {} has more than one constraint",
390 constraint.node_index
391 ));
392 }
393
394 let source = constraint_source(constraint)
395 .expect("every public constraint variant has a source node");
396 let weight = match &constraint.constraint {
397 NodeConstraintKind::Roll(value) => value.weight,
398 NodeConstraintKind::Aim(value) => value.weight,
399 NodeConstraintKind::Rotation(value) => value.weight,
400 };
401 if source as usize >= node_count || source == constraint.node_index {
402 return Err(format!(
403 "constraint on node {} has invalid source node {source}",
404 constraint.node_index
405 ));
406 }
407 if !weight.is_finite() || !(0.0..=1.0).contains(&weight) {
408 return Err(format!(
409 "constraint on node {} has a weight outside 0..=1",
410 constraint.node_index
411 ));
412 }
413 }
414
415 if dependency_order(constraints).len() != constraints.len() {
416 return Err("node constraints contain a circular dependency".to_string());
417 }
418 Ok(())
419}
420
421fn constraint_source(constraint: &NodeConstraint) -> Option<u32> {
422 match &constraint.constraint {
423 NodeConstraintKind::Roll(roll) => Some(roll.source),
424 NodeConstraintKind::Aim(aim) => Some(aim.source),
425 NodeConstraintKind::Rotation(rotation) => Some(rotation.source),
426 }
427}
428
429fn roll_axis_vec(axis: RollAxis) -> Vec3 {
430 match axis {
431 RollAxis::X => Vec3::X,
432 RollAxis::Y => Vec3::Y,
433 RollAxis::Z => Vec3::Z,
434 }
435}
436
437fn aim_axis_vec(axis: AimAxis) -> Vec3 {
438 match axis {
439 AimAxis::PositiveX => Vec3::X,
440 AimAxis::NegativeX => Vec3::NEG_X,
441 AimAxis::PositiveY => Vec3::Y,
442 AimAxis::NegativeY => Vec3::NEG_Y,
443 AimAxis::PositiveZ => Vec3::Z,
444 AimAxis::NegativeZ => Vec3::NEG_Z,
445 }
446}
447
448fn compute_world(nodes: &[AvatarNode], local_rotations: &[Quat]) -> Vec<Mat4> {
451 let local_matrices = nodes
452 .iter()
453 .zip(local_rotations)
454 .map(|(node, &rotation)| {
455 Mat4::from_scale_rotation_translation(
456 node.initial_scale,
457 rotation,
458 node.initial_translation,
459 )
460 })
461 .collect::<Vec<_>>();
462 crate::node::compute_world_from_local_matrices(nodes, &local_matrices)
463}
464
465#[cfg(test)]
466mod tests {
467 use super::*;
468
469 fn node(index: u32, translation: Vec3, parent: Option<u32>) -> AvatarNode {
470 AvatarNode {
471 index,
472 name: None,
473 parent,
474 children: Vec::new(),
475 initial_translation: translation,
476 initial_rotation: Quat::IDENTITY,
477 initial_scale: Vec3::ONE,
478 }
479 }
480
481 fn world_from(avatar: &Avatar, updates: &[BoneUpdate]) -> Vec<Mat4> {
482 avatar.compute_world_matrices(updates)
483 }
484
485 fn solved(avatar: &Avatar, world: &[Mat4]) -> Vec<BoneUpdate> {
486 solve_constraints(avatar, world)
487 }
488
489 #[test]
490 fn no_constraints_yield_no_updates() {
491 let avatar = Avatar::new().with_nodes(vec![node(0, Vec3::ZERO, None)]);
492 let world = world_from(&avatar, &[]);
493 assert!(solved(&avatar, &world).is_empty());
494 }
495
496 #[test]
497 fn rotation_constraint_copies_source_local_rotation() {
498 let avatar =
499 Avatar::new().with_nodes(vec![node(0, Vec3::ZERO, None), node(1, Vec3::ZERO, None)]);
500 let avatar = avatar.with_node_constraints(vec![NodeConstraint {
501 node_index: 1,
502 constraint: NodeConstraintKind::Rotation(RotationConstraint {
503 source: 0,
504 weight: 1.0,
505 }),
506 }]);
507
508 let source_delta = Quat::from_rotation_z(std::f32::consts::FRAC_PI_2);
509 let world = world_from(&avatar, &[BoneUpdate::new(0, source_delta)]);
510
511 let updates = solved(&avatar, &world);
512 assert_eq!(updates.len(), 1);
513 assert_eq!(updates[0].node_index(), 1);
514 assert!(
515 updates[0].rotation().angle_between(source_delta) < 5e-3,
516 "destination should copy the source rotation"
517 );
518 }
519
520 #[test]
521 fn rotation_constraint_weight_slerps() {
522 let avatar =
523 Avatar::new().with_nodes(vec![node(0, Vec3::ZERO, None), node(1, Vec3::ZERO, None)]);
524 let avatar = avatar.with_node_constraints(vec![NodeConstraint {
525 node_index: 1,
526 constraint: NodeConstraintKind::Rotation(RotationConstraint {
527 source: 0,
528 weight: 0.5,
529 }),
530 }]);
531
532 let source_delta = Quat::from_rotation_z(std::f32::consts::FRAC_PI_2);
533 let world = world_from(&avatar, &[BoneUpdate::new(0, source_delta)]);
534
535 let updates = solved(&avatar, &world);
536 assert_eq!(updates.len(), 1);
537 let expected = Quat::from_rotation_z(std::f32::consts::FRAC_PI_4);
538 assert!(
539 updates[0].rotation().angle_between(expected) < 5e-3,
540 "weight 0.5 should produce the halfway rotation"
541 );
542 }
543
544 #[test]
545 fn aim_constraint_points_axis_at_source() {
546 let avatar =
547 Avatar::new().with_nodes(vec![node(0, Vec3::ZERO, None), node(1, Vec3::Y, None)]);
548 let avatar = avatar.with_node_constraints(vec![NodeConstraint {
549 node_index: 0,
550 constraint: NodeConstraintKind::Aim(AimConstraint {
551 source: 1,
552 aim_axis: AimAxis::PositiveZ,
553 weight: 1.0,
554 }),
555 }]);
556
557 let world = world_from(&avatar, &[]);
558 let updates = solved(&avatar, &world);
559
560 assert_eq!(updates.len(), 1);
561 assert_eq!(updates[0].node_index(), 0);
562 let aimed = updates[0].rotation() * Vec3::Z;
563 assert!(
564 aimed.distance(Vec3::Y) < 1e-3,
565 "the +Z axis should point at the source, got {aimed:?}"
566 );
567 }
568
569 #[test]
570 fn roll_constraint_transfers_axis_rotation() {
571 let avatar =
572 Avatar::new().with_nodes(vec![node(0, Vec3::ZERO, None), node(1, Vec3::ZERO, None)]);
573 let avatar = avatar.with_node_constraints(vec![NodeConstraint {
574 node_index: 1,
575 constraint: NodeConstraintKind::Roll(RollConstraint {
576 source: 0,
577 roll_axis: RollAxis::X,
578 weight: 1.0,
579 }),
580 }]);
581
582 let source_delta = Quat::from_rotation_x(std::f32::consts::FRAC_PI_2);
583 let world = world_from(&avatar, &[BoneUpdate::new(0, source_delta)]);
584
585 let updates = solved(&avatar, &world);
586 assert_eq!(updates.len(), 1);
587 assert_eq!(updates[0].node_index(), 1);
588 assert!(
589 updates[0].rotation().angle_between(source_delta) < 5e-3,
590 "the roll around X should be copied to the destination"
591 );
592 }
593
594 #[test]
595 fn self_referencing_constraint_is_ignored() {
596 let avatar = Avatar::new().with_nodes(vec![node(0, Vec3::ZERO, None)]);
597 let avatar = avatar.with_node_constraints(vec![NodeConstraint {
598 node_index: 0,
599 constraint: NodeConstraintKind::Rotation(RotationConstraint {
600 source: 0,
601 weight: 1.0,
602 }),
603 }]);
604
605 let source_delta = Quat::from_rotation_z(std::f32::consts::FRAC_PI_2);
606 let world = world_from(&avatar, &[BoneUpdate::new(0, source_delta)]);
607 assert!(solved(&avatar, &world).is_empty());
608 }
609
610 #[test]
611 fn circular_dependencies_are_skipped() {
612 let avatar = Avatar::new().with_nodes(vec![
613 node(0, Vec3::ZERO, None),
614 node(1, Vec3::ZERO, None),
615 node(2, Vec3::ZERO, None),
616 ]);
617 let avatar = avatar.with_node_constraints(vec![
618 NodeConstraint {
619 node_index: 1,
620 constraint: NodeConstraintKind::Rotation(RotationConstraint {
621 source: 2,
622 weight: 1.0,
623 }),
624 },
625 NodeConstraint {
626 node_index: 2,
627 constraint: NodeConstraintKind::Rotation(RotationConstraint {
628 source: 1,
629 weight: 1.0,
630 }),
631 },
632 ]);
633
634 let source_delta = Quat::from_rotation_z(std::f32::consts::FRAC_PI_2);
635 let world = world_from(&avatar, &[BoneUpdate::new(0, source_delta)]);
636 assert!(solved(&avatar, &world).is_empty());
637 }
638
639 #[test]
640 fn dependency_chain_resolves_in_order() {
641 let avatar = Avatar::new().with_nodes(vec![
642 node(0, Vec3::ZERO, None),
643 node(1, Vec3::ZERO, None),
644 node(2, Vec3::ZERO, None),
645 ]);
646 let avatar = avatar.with_node_constraints(vec![
647 NodeConstraint {
648 node_index: 1,
649 constraint: NodeConstraintKind::Rotation(RotationConstraint {
650 source: 0,
651 weight: 1.0,
652 }),
653 },
654 NodeConstraint {
655 node_index: 2,
656 constraint: NodeConstraintKind::Rotation(RotationConstraint {
657 source: 1,
658 weight: 1.0,
659 }),
660 },
661 ]);
662
663 let source_delta = Quat::from_rotation_z(std::f32::consts::FRAC_PI_2);
664 let world = world_from(&avatar, &[BoneUpdate::new(0, source_delta)]);
665
666 let updates = solved(&avatar, &world);
667 assert_eq!(updates.len(), 2);
668 for update in &updates {
671 assert!(
672 update.rotation().angle_between(source_delta) < 5e-3,
673 "node {} should copy the source rotation",
674 update.node_index()
675 );
676 }
677 }
678}