1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
use crate::engine::ecs::component::TransformComponent;
use crate::engine::ecs::{ComponentId, World};
use crate::utils::math::{vec3_len, vec3_sub};
/// Stateless utility for resolving semantic bone landmarks to ComponentIds in a live skeleton.
///
/// All functions are free (associated) functions — no instance state.
/// Called once during `AvatarControlSystem::try_init_splices`; returns resolved IDs
/// that AVC uses to wire IK chains.
pub struct BoneMappingSystem;
/// Resolved upper-arm → lower-arm → hand chain for TwoBoneIK setup.
pub struct ResolvedArmChain {
pub upper_arm: ComponentId,
pub lower_arm: ComponentId,
pub hand: ComponentId,
}
/// Resolved spine chain for FABRIK setup.
///
/// `chain` is ordered hips → ... → head (root-first FABRIK convention).
/// Intermediate joints (spine/chest/upper_chest/neck) are whatever TC ancestors
/// the topology walk produced between hips and head — count varies by rig.
pub struct ResolvedSpineChain {
pub hips: ComponentId,
pub head: ComponentId,
pub chain: Vec<ComponentId>,
}
impl BoneMappingSystem {
/// Resolve a 2-bone arm chain from (optional) explicit names + topology fallback.
///
/// Resolution order for each joint:
/// 1. Explicit name, if provided — look up by `#name` MMQ selector under `model_root`.
/// 2. Topology derivation — walk TC parent chain from the joint below, using
/// `tc_ancestor_at_distance` with the given `min_bone_length` threshold.
///
/// `min_bone_length`: if `Some(d)`, topology derivation skips TC ancestors closer
/// than `d` metres to the previous anchor, filtering out short helper bones.
/// Pass `None` to always use the immediate TC parent.
///
/// Returns `None` if `hand_name` is not found under `model_root`.
pub fn resolve_arm_chain(
world: &World,
model_root: ComponentId,
hand_name: &str,
lower_arm_name: Option<&str>,
upper_arm_name: Option<&str>,
min_bone_length: Option<f32>,
) -> Option<ResolvedArmChain> {
let hand = world.find_component(model_root, &format!("#{}", hand_name))?;
let lower_arm = if let Some(name) = lower_arm_name {
world.find_component(model_root, &format!("#{}", name))?
} else {
Self::tc_ancestor_at_distance(world, hand, min_bone_length)?
};
let upper_arm = if let Some(name) = upper_arm_name {
world.find_component(model_root, &format!("#{}", name))?
} else {
Self::tc_ancestor_at_distance(world, lower_arm, min_bone_length)?
};
Some(ResolvedArmChain {
upper_arm,
lower_arm,
hand,
})
}
/// Resolve a spine chain from head bone up to (optionally named) hips bone.
///
/// Walks UP from `head_id` via `tc_ancestor_at_distance` (threshold ~0.03m to
/// skip helper bones), collecting TC joints. Stops when it hits `hips_name`
/// (by component name) if provided, or after at most 8 hops otherwise.
///
/// Returns the chain in HIPS → HEAD order (FABRIK convention: root first).
/// Returns `None` if the walk produces fewer than 2 joints.
pub fn resolve_spine_chain(
world: &World,
model_root: ComponentId,
head_id: ComponentId,
hips_name: Option<&str>,
min_bone_length: Option<f32>,
) -> Option<ResolvedSpineChain> {
let hips_id = hips_name.and_then(|n| world.find_component(model_root, &format!("#{}", n)));
let mut up: Vec<ComponentId> = vec![head_id];
let mut cur = head_id;
for _ in 0..8 {
let parent = Self::tc_ancestor_at_distance(world, cur, min_bone_length)?;
up.push(parent);
// Stop if we hit the named hips.
if Some(parent) == hips_id {
break;
}
// Or stop if we've stepped above model_root.
if parent == model_root {
break;
}
cur = parent;
}
if up.len() < 2 {
return None;
}
up.reverse();
let hips = *up.first().unwrap();
let head = *up.last().unwrap();
Some(ResolvedSpineChain {
hips,
head,
chain: up,
})
}
/// Walk upward from `start`, returning the nearest TC ancestor whose world position
/// is at least `min_dist` metres away from `start`.
///
/// If `min_dist` is `None`, returns the immediate TC parent (no distance filtering).
/// Returns `None` if no TC parent is found, or if the walk exceeds 32 steps.
pub fn tc_ancestor_at_distance(
world: &World,
start: ComponentId,
min_dist: Option<f32>,
) -> Option<ComponentId> {
let anchor_pos = tc_world_pos(world, start)?;
let mut cur = start;
for _ in 0..32 {
let parent = world.parent_of(cur)?;
// Parent must be a TC to count as an arm joint.
if world
.get_component_by_id_as::<TransformComponent>(parent)
.is_none()
{
return None;
}
if let Some(min_d) = min_dist {
let parent_pos = tc_world_pos(world, parent)?;
let dist = vec3_len(vec3_sub(parent_pos, anchor_pos));
if dist < min_d {
// Too close — this is a helper bone; keep walking.
cur = parent;
continue;
}
}
return Some(parent);
}
None
}
/// Find the first TC ancestor of `start` that has >= `min_tc_children` TC children.
///
/// Used for shoulder girdle detection (first 3-way split above hips) and hip detection.
/// Returns `None` if no such ancestor exists within 32 steps.
pub fn find_branching_ancestor(
world: &World,
start: ComponentId,
min_tc_children: usize,
) -> Option<ComponentId> {
let mut cur = start;
for _ in 0..32 {
let parent = world.parent_of(cur)?;
if world
.get_component_by_id_as::<TransformComponent>(parent)
.is_none()
{
return None;
}
let tc_child_count = world
.children_of(parent)
.iter()
.filter(|&&ch| {
world
.get_component_by_id_as::<TransformComponent>(ch)
.is_some()
})
.count();
if tc_child_count >= min_tc_children {
return Some(parent);
}
cur = parent;
}
None
}
}
// ---------------------------------------------------------------------------
// Private helpers
// ---------------------------------------------------------------------------
fn tc_world_pos(world: &World, id: ComponentId) -> Option<[f32; 3]> {
world
.get_component_by_id_as::<TransformComponent>(id)
.map(|t| {
let m = t.transform.matrix_world;
[m[3][0], m[3][1], m[3][2]]
})
}