mittens_engine/engine/ecs/system/
secondary_motion_system.rs

1use crate::engine::ecs::component::{
2    BoneRestPoseComponent, ComponentRef, GLTFComponent, QueryRootMode, SecondaryMotionComponent,
3    SpringBoneComponent, SpringJointComponent, TransformComponent, resolve_component_ref,
4};
5use crate::engine::ecs::{ComponentId, World};
6use crate::utils::math::{
7    mat_to_quat, quat_conjugate, quat_mul, quat_rotate_vec3, shortest_arc_quat, vec3_add, vec3_len,
8    vec3_normalize, vec3_scale, vec3_sub,
9};
10use std::collections::{HashMap, HashSet};
11
12const STEP: f32 = 1.0 / 60.0;
13
14#[derive(Debug, Clone)]
15struct JointConfig {
16    id: ComponentId,
17    rest_rotation: [f32; 4],
18    stiffness: f32,
19    drag: f32,
20    gravity: [f32; 3],
21}
22#[derive(Debug, Clone)]
23struct ChainState {
24    gltf: ComponentId,
25    joints: Vec<JointConfig>,
26    previous: Vec<[f32; 3]>,
27    current: Vec<[f32; 3]>,
28    lengths: Vec<f32>,
29    accumulator: f32,
30    enabled: bool,
31}
32
33#[derive(Debug, Default)]
34pub struct SecondaryMotionSystem {
35    states: HashMap<ComponentId, ChainState>,
36    warned: HashSet<ComponentId>,
37    failures: HashMap<ComponentId, String>,
38    debug_frames: u64,
39}
40
41impl SecondaryMotionSystem {
42    pub fn reset(&mut self, target: ComponentId) {
43        self.states
44            .retain(|chain, state| *chain != target && state.gltf != target);
45    }
46
47    pub fn tick(&mut self, world: &mut World, dt: f32) {
48        self.debug_frames = self.debug_frames.wrapping_add(1);
49        let mut max_correction_radians = 0.0f32;
50        let roots: Vec<_> = world
51            .all_components()
52            .filter(|id| {
53                world
54                    .get_component_by_id_as::<SecondaryMotionComponent>(*id)
55                    .is_some()
56            })
57            .collect();
58        let mut live = HashSet::new();
59        for root in roots {
60            let Some(gltf_id) = nearest_gltf(world, root) else {
61                continue;
62            };
63            let mut owned = HashSet::new();
64            let chains: Vec<_> = world
65                .children_of(root)
66                .iter()
67                .copied()
68                .filter(|id| {
69                    world
70                        .get_component_by_id_as::<SpringBoneComponent>(*id)
71                        .is_some()
72                })
73                .collect();
74            for chain_id in chains {
75                live.insert(chain_id);
76                if !self.states.contains_key(&chain_id) {
77                    match bind_chain(world, gltf_id, chain_id, &mut owned) {
78                        Ok(state) => {
79                            self.states.insert(chain_id, state);
80                            self.warned.remove(&chain_id);
81                            self.failures.remove(&chain_id);
82                        }
83                        Err(error) => {
84                            self.failures.insert(chain_id, error.clone());
85                            if self.warned.insert(chain_id) {
86                                eprintln!(
87                                    "[SecondaryMotion] chain {chain_id:?}: {error}; will retry after respawn"
88                                );
89                            }
90                        }
91                    }
92                }
93                let Some(state) = self.states.get_mut(&chain_id) else {
94                    continue;
95                };
96                if state
97                    .joints
98                    .iter()
99                    .any(|j| world.get_component_record(j.id).is_none())
100                {
101                    self.states.remove(&chain_id);
102                    continue;
103                }
104                let enabled = world
105                    .get_component_by_id_as::<SpringBoneComponent>(chain_id)
106                    .map(|c| c.enabled)
107                    .unwrap_or(false);
108                if !enabled {
109                    state.enabled = false;
110                    continue;
111                }
112                if !state.enabled || !dt.is_finite() || dt > 0.25 {
113                    reset_state(world, state);
114                    state.enabled = true;
115                    continue;
116                }
117                state.accumulator = (state.accumulator + dt.max(0.0)).min(STEP * 4.0);
118                while state.accumulator >= STEP {
119                    simulate_step(world, state);
120                    state.accumulator -= STEP;
121                }
122                max_correction_radians = max_correction_radians.max(apply_rotations(world, state));
123            }
124        }
125        self.states.retain(|id, _| live.contains(id));
126        self.failures.retain(|id, _| live.contains(id));
127        if self.debug_frames % 120 == 0 && std::env::var_os("CAT_DEBUG_SECONDARY_MOTION").is_some()
128        {
129            eprintln!(
130                "[SecondaryMotion][debug] bound_chains={} failed_chains={} max_rotation_correction_deg={:.2}",
131                self.states.len(),
132                self.failures.len(),
133                max_correction_radians.to_degrees()
134            );
135            for (chain_id, error) in self.failures.iter().take(3) {
136                let name = world
137                    .get_component_by_id_as::<SpringBoneComponent>(*chain_id)
138                    .map(|chain| chain.stable_name.as_str())
139                    .unwrap_or("<removed>");
140                eprintln!("[SecondaryMotion][debug] failed '{name}': {error}");
141            }
142        }
143    }
144}
145
146fn nearest_gltf(world: &World, mut id: ComponentId) -> Option<ComponentId> {
147    for _ in 0..64 {
148        id = world.parent_of(id)?;
149        if world.get_component_by_id_as::<GLTFComponent>(id).is_some() {
150            return Some(id);
151        }
152    }
153    None
154}
155fn descendant(world: &World, mut node: ComponentId, ancestor: ComponentId) -> bool {
156    for _ in 0..64 {
157        if node == ancestor {
158            return true;
159        }
160        let Some(p) = world.parent_of(node) else {
161            return false;
162        };
163        node = p;
164    }
165    false
166}
167fn pos(world: &World, id: ComponentId) -> Option<[f32; 3]> {
168    let m = world
169        .get_component_by_id_as::<TransformComponent>(id)?
170        .transform
171        .matrix_world;
172    Some([m[3][0], m[3][1], m[3][2]])
173}
174fn rest(world: &World, id: ComponentId) -> Option<BoneRestPoseComponent> {
175    world.children_of(id).iter().find_map(|c| {
176        world
177            .get_component_by_id_as::<BoneRestPoseComponent>(*c)
178            .copied()
179    })
180}
181
182fn ref_surface(reference: &ComponentRef) -> String {
183    match reference {
184        ComponentRef::Guid(guid) => format!("@uuid:{guid}"),
185        ComponentRef::Query(query) => query.clone(),
186    }
187}
188
189fn resolve_in_gltf(
190    world: &World,
191    gltf_id: ComponentId,
192    reference: &ComponentRef,
193) -> Result<ComponentId, String> {
194    let gltf = world
195        .get_component_by_id_as::<GLTFComponent>(gltf_id)
196        .ok_or("owning GLTF disappeared")?;
197    let instance_nodes: HashSet<_> = gltf.spawned_node_transforms.iter().copied().collect();
198    if instance_nodes.is_empty() {
199        return Err("owning GLTF has not spawned its node transforms yet".into());
200    }
201    let anchor = world
202        .parent_of(gltf_id)
203        .ok_or("owning GLTF has no transform anchor")?;
204
205    let id = match reference {
206        ComponentRef::Guid(guid) => world.component_id_by_guid(*guid),
207        ComponentRef::Query(query) if !query.starts_with('/') && !query.starts_with("../") => {
208            // Imported nodes can be reparented after spawning (AvatarControl
209            // splices the head subtree, including hair). Match against the
210            // GLTF instance's recorded node set instead of assuming those
211            // nodes remain reachable below the original anchor.
212            let matches: Vec<_> = instance_nodes
213                .iter()
214                .copied()
215                .filter(|id| world.component_matches_selector(*id, query))
216                .collect();
217            if matches.len() != 1 {
218                return Err(format!(
219                    "query '{}' matched {} nodes in the owning GLTF instance (expected exactly one)",
220                    query,
221                    matches.len()
222                ));
223            }
224            matches.first().copied()
225        }
226        ComponentRef::Query(_) => resolve_component_ref(
227            world,
228            reference,
229            Some(anchor),
230            QueryRootMode::SelfSubtree,
231        ),
232    }
233    .ok_or_else(|| format!("reference '{}' did not resolve", ref_surface(reference)))?;
234
235    if !instance_nodes.contains(&id) {
236        return Err(format!(
237            "reference '{}' resolved outside the owning GLTF instance",
238            ref_surface(reference)
239        ));
240    }
241    Ok(id)
242}
243
244fn bind_chain(
245    world: &World,
246    gltf_id: ComponentId,
247    chain_id: ComponentId,
248    owned: &mut HashSet<ComponentId>,
249) -> Result<ChainState, String> {
250    if let Some(anchor) = world
251        .parent_of(gltf_id)
252        .and_then(|id| world.get_component_by_id_as::<TransformComponent>(id))
253    {
254        let m = anchor.transform.matrix_world;
255        let sx = vec3_len([m[0][0], m[0][1], m[0][2]]);
256        let sy = vec3_len([m[1][0], m[1][1], m[1][2]]);
257        let sz = vec3_len([m[2][0], m[2][1], m[2][2]]);
258        let det = m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
259            - m[1][0] * (m[0][1] * m[2][2] - m[0][2] * m[2][1])
260            + m[2][0] * (m[0][1] * m[1][2] - m[0][2] * m[1][1]);
261        if det <= 0.0 || (sx - sy).abs() > 1e-4 || (sx - sz).abs() > 1e-4 {
262            return Err("non-uniform or negative GLTF instance scale is unsupported".into());
263        }
264    }
265    let chain = world
266        .get_component_by_id_as::<SpringBoneComponent>(chain_id)
267        .ok_or("missing SpringBone")?;
268    if world
269        .get_component_by_id_as::<GLTFComponent>(gltf_id)
270        .is_none()
271    {
272        return Err("owning GLTF disappeared".into());
273    }
274    if let Some(center) = &chain.center {
275        resolve_in_gltf(world, gltf_id, center).map_err(|error| format!("center {error}"))?;
276    }
277    let joint_components: Vec<_> = world
278        .children_of(chain_id)
279        .iter()
280        .filter_map(|id| {
281            world
282                .get_component_by_id_as::<SpringJointComponent>(*id)
283                .map(|j| (*id, j))
284        })
285        .collect();
286    if joint_components.is_empty() {
287        return Err("has no SpringJoint children".into());
288    }
289    let mut joints = Vec::new();
290    for (_, j) in joint_components {
291        let id = resolve_in_gltf(world, gltf_id, &j.node)?;
292        if !owned.insert(id) {
293            return Err(format!(
294                "joint '{}' overlaps another chain",
295                ref_surface(&j.node)
296            ));
297        }
298        let r = rest(world, id)
299            .ok_or_else(|| format!("joint '{}' has no rest pose", ref_surface(&j.node)))?;
300        joints.push(JointConfig {
301            id,
302            rest_rotation: r.rotation,
303            stiffness: j.stiffness,
304            drag: j.drag_force,
305            gravity: vec3_scale(vec3_normalize(j.gravity_dir), j.gravity_power),
306        });
307    }
308    for pair in joints.windows(2) {
309        if !descendant(world, pair[1].id, pair[0].id) {
310            return Err("joint list is reordered or non-ancestral".into());
311        }
312    }
313    let virtual_ratio = chain.virtual_end_length_ratio;
314    if joints.len() == 1 && virtual_ratio.is_none() {
315        return Err("a one-joint chain requires a virtual endpoint".into());
316    }
317    let mut current = Vec::new();
318    let mut lengths = Vec::new();
319    for i in 0..joints.len() {
320        let head = pos(world, joints[i].id).ok_or("joint transform missing")?;
321        let tail = if i + 1 < joints.len() {
322            pos(world, joints[i + 1].id).unwrap()
323        } else {
324            let ratio = virtual_ratio.unwrap_or(1.0);
325            let prev = pos(world, joints[i - 1].id).unwrap();
326            vec3_add(
327                head,
328                vec3_scale(
329                    vec3_normalize(vec3_sub(head, prev)),
330                    vec3_len(vec3_sub(head, prev)) * ratio,
331                ),
332            )
333        };
334        lengths.push(vec3_len(vec3_sub(tail, head)));
335        current.push(tail);
336    }
337    Ok(ChainState {
338        gltf: gltf_id,
339        joints,
340        previous: current.clone(),
341        current,
342        lengths,
343        accumulator: 0.0,
344        enabled: chain.enabled,
345    })
346}
347fn reset_state(world: &World, s: &mut ChainState) {
348    for i in 0..s.joints.len() {
349        let head = pos(world, s.joints[i].id).unwrap_or([0.0; 3]);
350        let tail = if i + 1 < s.joints.len() {
351            pos(world, s.joints[i + 1].id).unwrap_or(head)
352        } else if i > 0 {
353            let prev = pos(world, s.joints[i - 1].id).unwrap_or(head);
354            vec3_add(
355                head,
356                vec3_scale(vec3_normalize(vec3_sub(head, prev)), s.lengths[i]),
357            )
358        } else {
359            head
360        };
361        s.current[i] = tail;
362        s.previous[i] = tail;
363    }
364    s.accumulator = 0.0;
365}
366fn simulate_step(world: &World, s: &mut ChainState) {
367    for i in 0..s.joints.len() {
368        // A joint's simulated tail is the next joint's simulated head. Using
369        // primary-pose heads for every segment breaks chain continuity and
370        // creates the characteristic curl/flicker feedback.
371        let primary_head = pos(world, s.joints[i].id).unwrap_or(s.current[i]);
372        let head = if i == 0 {
373            primary_head
374        } else {
375            s.current[i - 1]
376        };
377        let inertia = vec3_scale(
378            vec3_sub(s.current[i], s.previous[i]),
379            1.0 - s.joints[i].drag,
380        );
381        let rest_tail = if i + 1 < s.joints.len() {
382            let primary_tail = pos(world, s.joints[i + 1].id).unwrap_or(s.current[i]);
383            vec3_add(
384                head,
385                vec3_scale(
386                    vec3_normalize(vec3_sub(primary_tail, primary_head)),
387                    s.lengths[i],
388                ),
389            )
390        } else if i > 0 {
391            let prev = pos(world, s.joints[i - 1].id).unwrap_or(head);
392            vec3_add(
393                head,
394                vec3_scale(vec3_normalize(vec3_sub(head, prev)), s.lengths[i]),
395            )
396        } else {
397            s.current[i]
398        };
399        let stiffness = vec3_scale(
400            vec3_sub(rest_tail, s.current[i]),
401            s.joints[i].stiffness * STEP,
402        );
403        let next = vec3_add(
404            vec3_add(s.current[i], inertia),
405            vec3_add(stiffness, vec3_scale(s.joints[i].gravity, STEP * STEP)),
406        );
407        s.previous[i] = s.current[i];
408        let direction = vec3_normalize(vec3_sub(next, head));
409        s.current[i] = vec3_add(head, vec3_scale(direction, s.lengths[i]));
410    }
411}
412fn apply_rotations(world: &mut World, s: &ChainState) -> f32 {
413    let mut max_correction = 0.0f32;
414    let mut previous_joint_world_q = None;
415    for i in 0..s.joints.len() {
416        let id = s.joints[i].id;
417        let Some(parent) = world.parent_of(id) else {
418            continue;
419        };
420        let parent_q = if i > 0 && parent == s.joints[i - 1].id {
421            previous_joint_world_q.unwrap_or([0.0, 0.0, 0.0, 1.0])
422        } else {
423            world
424                .get_component_by_id_as::<TransformComponent>(parent)
425                .map(|t| mat_to_quat(t.transform.matrix_world))
426                .unwrap_or([0.0, 0.0, 0.0, 1.0])
427        };
428        let head = if i == 0 {
429            let Some(head) = pos(world, id) else { continue };
430            head
431        } else {
432            s.current[i - 1]
433        };
434        let desired_local = quat_rotate_vec3(
435            quat_conjugate(parent_q),
436            vec3_normalize(vec3_sub(s.current[i], head)),
437        );
438        let rest_dir = if i + 1 < s.joints.len() {
439            rest(world, s.joints[i + 1].id)
440                // Child translation is in the joint's local frame. Convert
441                // it through the immutable rest rotation before comparing it
442                // with a direction expressed in the parent frame.
443                .map(|r| quat_rotate_vec3(s.joints[i].rest_rotation, vec3_normalize(r.translation)))
444                .unwrap_or(desired_local)
445        } else if i > 0 {
446            rest(world, id)
447                .map(|r| vec3_normalize(r.translation))
448                .unwrap_or(desired_local)
449        } else {
450            desired_local
451        };
452        let correction = shortest_arc_quat(rest_dir, desired_local);
453        max_correction = max_correction.max(2.0 * correction[3].abs().clamp(0.0, 1.0).acos());
454        let rotation = quat_mul(correction, s.joints[i].rest_rotation);
455        previous_joint_world_q = Some(quat_mul(parent_q, rotation));
456        if let Some(t) = world.get_component_by_id_as_mut::<TransformComponent>(id) {
457            t.transform.rotation = rotation;
458            t.transform.recompute_model();
459        }
460    }
461    max_correction
462}
mittens_engine/engine/ecs/system/secondary_motion_system.rs

mittens_engine/engine/ecs/system/
secondary_motion_system.rs
