mmd_anim_runtime/
runtime.rs

1use std::sync::Arc;
2
3use glam::{Mat4, Quat};
4
5use crate::{AnimationClip, ModelArena, MorphIndex, PoseArena};
6use crate::{
7    append_primitive::{AppendPrimitiveInput, solve_append_transform},
8    ik_primitive::{
9        ChainLinkState, LinkStepInput, constrain_rotation_to_axis, rotation, solve_link_step,
10        translation,
11    },
12};
13
14#[cfg(test)]
15use crate::ik_primitive::{
16    LimitedAxesLinkStepInput, PlaneLinkStepInput, axis_vec, decompose_euler_xyz, euler_xyz_to_quat,
17    limit_axis_bounds, quat_to_rotation_mat3, signed_projected_angle, solve_limited_axes_link_step,
18    solve_plane_link_step,
19};
20
21#[derive(Debug)]
22struct IkScratch {
23    links: Vec<crate::IkLink>,
24    base_rotations: Vec<Quat>,
25    ik_rotations: Vec<Quat>,
26    best_ik_rotations: Vec<Quat>,
27    chain_states: Vec<ChainLinkState>,
28}
29
30impl IkScratch {
31    fn new(model: &ModelArena) -> Self {
32        let max_links = model
33            .ik_solvers()
34            .iter()
35            .map(|s| s.links.len())
36            .max()
37            .unwrap_or(0);
38        IkScratch {
39            links: Vec::with_capacity(max_links),
40            base_rotations: Vec::with_capacity(max_links),
41            ik_rotations: Vec::with_capacity(max_links),
42            best_ik_rotations: Vec::with_capacity(max_links),
43            chain_states: Vec::with_capacity(max_links),
44        }
45    }
46}
47
48#[derive(Debug)]
49struct MorphScratch {
50    expanded_weights: Vec<f32>,
51}
52
53impl MorphScratch {
54    fn new(morph_count: usize) -> Self {
55        Self {
56            expanded_weights: vec![0.0; morph_count],
57        }
58    }
59}
60
61#[derive(Clone, Copy, Debug, Default, PartialEq)]
62pub struct IkSolverRuntimeStats {
63    pub solver_evaluations: u64,
64    pub configured_iterations: u64,
65    pub executed_iterations: u64,
66    pub tolerance_precheck_breaks: u64,
67    pub tolerance_post_iteration_breaks: u64,
68    pub rollback_breaks: u64,
69    pub max_iteration_exhaustions: u64,
70    pub link_visits: u64,
71    pub link_steps: u64,
72    pub final_distance_sum: f64,
73    pub final_distance_max: f32,
74    pub exhausted_final_distance_sum: f64,
75    pub exhausted_final_distance_max: f32,
76}
77
78impl IkSolverRuntimeStats {
79    fn reset(&mut self) {
80        *self = Self::default();
81    }
82}
83
84#[derive(Clone, Copy, Debug, PartialEq)]
85pub struct IkSolveOptions {
86    pub tolerance: f32,
87    pub max_iterations_cap: Option<u32>,
88}
89
90impl Default for IkSolveOptions {
91    fn default() -> Self {
92        Self {
93            tolerance: 1.0e-2,
94            max_iterations_cap: None,
95        }
96    }
97}
98
99#[derive(Debug)]
100pub struct RuntimeInstance {
101    model: Arc<ModelArena>,
102    pose: PoseArena,
103    ik_scratch: IkScratch,
104    morph_scratch: MorphScratch,
105    ik_stats: Vec<IkSolverRuntimeStats>,
106    #[cfg(test)]
107    world_matrix_bone_update_count: usize,
108}
109
110impl RuntimeInstance {
111    pub fn new(model: Arc<ModelArena>) -> Self {
112        let morph_count = model.morph_count() as usize;
113        Self::new_with_morph_count(model, morph_count)
114    }
115
116    pub fn new_with_morph_count(model: Arc<ModelArena>, morph_count: usize) -> Self {
117        let ik_count = model.ik_count();
118        Self::new_with_counts(model, morph_count, ik_count)
119    }
120
121    pub fn new_with_counts(model: Arc<ModelArena>, morph_count: usize, ik_count: usize) -> Self {
122        let morph_count = morph_count.max(model.morph_count() as usize);
123        let pose = PoseArena::new_with_counts(model.bone_count(), morph_count, ik_count);
124        let ik_scratch = IkScratch::new(&model);
125        let morph_scratch = MorphScratch::new(morph_count);
126        let ik_stats = vec![IkSolverRuntimeStats::default(); model.ik_count()];
127        Self {
128            model,
129            pose,
130            ik_scratch,
131            morph_scratch,
132            ik_stats,
133            #[cfg(test)]
134            world_matrix_bone_update_count: 0,
135        }
136    }
137
138    #[inline]
139    pub fn model(&self) -> &ModelArena {
140        &self.model
141    }
142
143    #[inline]
144    pub fn pose(&self) -> &PoseArena {
145        &self.pose
146    }
147
148    #[inline]
149    pub fn pose_mut(&mut self) -> &mut PoseArena {
150        &mut self.pose
151    }
152
153    pub fn evaluate_current_pose(&mut self) {
154        self.update_world_matrices();
155        self.solve_enabled_ik(IkSolveOptions::default());
156    }
157
158    pub fn evaluate_current_pose_with_ik_options(&mut self, options: IkSolveOptions) {
159        self.update_world_matrices();
160        self.solve_enabled_ik(options);
161    }
162
163    /// Evaluate the current pose by updating world matrices only, without
164    /// running any IK solver. This is useful for diagnostics that need to
165    /// inspect clip/VMD state before IK is applied.
166    pub fn evaluate_current_pose_without_ik(&mut self) {
167        self.update_world_matrices();
168    }
169
170    fn update_world_matrices(&mut self) {
171        self.update_world_matrices_from_eval_order_position(0);
172    }
173
174    fn update_world_matrices_from_bone(&mut self, bone: crate::BoneIndex) {
175        self.update_world_matrices_from_eval_order_position(self.model.eval_order_position(bone));
176    }
177
178    fn update_world_matrices_from_eval_order_position(&mut self, start_position: usize) {
179        let start_position = self.expand_update_start_for_append_dependencies(start_position);
180        for bone in &self.model.eval_order()[start_position..] {
181            self.pose.reset_append_transform(*bone);
182        }
183        for bone in &self.model.eval_order()[start_position..] {
184            #[cfg(test)]
185            {
186                self.world_matrix_bone_update_count += 1;
187            }
188            let mut local_position =
189                self.model.rest_position(*bone) + self.pose.local_position_offset(*bone);
190            let mut local_rotation = self.pose.local_rotation(*bone);
191            let local_scale = self.pose.local_scale(*bone);
192
193            if let Some(append_index) = self.model.append_transform_index(*bone) {
194                let append = self.model.append_transform(append_index);
195                let use_source_append = !append.local
196                    && self
197                        .model
198                        .append_transform_index(append.source_bone)
199                        .is_some();
200                let source_rotation = if use_source_append {
201                    self.pose.append_rotation(append.source_bone)
202                } else {
203                    self.pose.local_rotation(append.source_bone)
204                };
205                let source_position_offset = if use_source_append {
206                    self.pose.append_position_offset(append.source_bone)
207                } else {
208                    self.pose.local_position_offset(append.source_bone)
209                };
210                let append_output = solve_append_transform(AppendPrimitiveInput {
211                    source_position_offset,
212                    source_rotation,
213                    ratio: append.ratio,
214                    affect_rotation: append.affect_rotation,
215                    affect_translation: append.affect_translation,
216                });
217                self.pose.set_append_rotation(*bone, append_output.rotation);
218                self.pose
219                    .set_append_position_offset(*bone, append_output.position_offset);
220                if append.affect_rotation {
221                    local_rotation = (local_rotation * append_output.rotation).normalize();
222                }
223                if append.affect_translation {
224                    local_position += append_output.position_offset;
225                }
226            }
227
228            if let Some(axis) = self.model.fixed_axis(*bone) {
229                local_rotation = constrain_rotation_to_axis(local_rotation, axis);
230            }
231
232            let local_matrix = Mat4::from_scale_rotation_translation(
233                local_scale.into(),
234                local_rotation,
235                local_position.into(),
236            );
237
238            let world_matrix = match self.model.parent_index(*bone) {
239                Some(parent) => self.pose.world_matrices()[parent.as_usize()] * local_matrix,
240                None => local_matrix,
241            };
242
243            self.pose.set_world_matrix(*bone, world_matrix);
244            self.pose
245                .set_skinning_matrix(*bone, world_matrix * self.model.inverse_bind_matrix(*bone));
246        }
247    }
248
249    fn expand_update_start_for_append_dependencies(&self, start_position: usize) -> usize {
250        let mut start = start_position;
251        loop {
252            let mut changed = false;
253            for append in self.model.append_transforms() {
254                let source_position = self.model.eval_order_position(append.source_bone);
255                let target_position = self.model.eval_order_position(append.target_bone);
256                if source_position >= start && target_position < start {
257                    start = target_position;
258                    changed = true;
259                }
260            }
261            if !changed {
262                return start;
263            }
264        }
265    }
266
267    fn min_link_eval_order_position(&self, links: &[crate::IkLink]) -> Option<usize> {
268        links
269            .iter()
270            .map(|link| self.model.eval_order_position(link.bone))
271            .min()
272    }
273
274    fn solve_enabled_ik(&mut self, options: IkSolveOptions) {
275        let tolerance = options.tolerance.max(0.0);
276        let mut links = std::mem::take(&mut self.ik_scratch.links);
277        let mut base_rotations = std::mem::take(&mut self.ik_scratch.base_rotations);
278        let mut ik_rotations = std::mem::take(&mut self.ik_scratch.ik_rotations);
279        let mut best_ik_rotations = std::mem::take(&mut self.ik_scratch.best_ik_rotations);
280        let mut chain_states = std::mem::take(&mut self.ik_scratch.chain_states);
281
282        for ik_index in 0..self.model.ik_count() {
283            if self.pose.ik_enabled()[ik_index] == 0 {
284                continue;
285            }
286
287            let solver = &self.model.ik_solvers()[ik_index];
288            let ik_bone = solver.ik_bone;
289            let target_bone = solver.target_bone;
290            let iteration_count = options
291                .max_iterations_cap
292                .map(|cap| solver.iteration_count.min(cap))
293                .unwrap_or(solver.iteration_count)
294                .max(1) as usize;
295            let limit_angle = solver.limit_angle.max(0.0);
296            let link_count = solver.links.len();
297
298            links.clear();
299            links.extend(solver.links.iter().cloned());
300            self.ik_stats[ik_index].solver_evaluations += 1;
301            self.ik_stats[ik_index].configured_iterations += iteration_count as u64;
302
303            base_rotations.clear();
304            base_rotations.extend(links.iter().map(|l| self.pose.local_rotation(l.bone)));
305            ik_rotations.clear();
306            ik_rotations.resize(link_count, Quat::IDENTITY);
307            best_ik_rotations.clear();
308            best_ik_rotations.resize(link_count, Quat::IDENTITY);
309            chain_states.clear();
310            chain_states.resize_with(link_count, || ChainLinkState {
311                previous_euler: [0.0; 3],
312                plane_mode_angle: 0.0,
313            });
314
315            // Always start from base rotations (IK deltas start at identity).
316            self.apply_ik_link_rotations(&links, &base_rotations, &ik_rotations);
317            if let Some(start_position) = self.min_link_eval_order_position(&links) {
318                self.update_world_matrices_from_eval_order_position(start_position);
319            } else {
320                self.update_world_matrices();
321            }
322
323            let mut broke_early = false;
324            let mut final_distance = f32::MAX;
325            let mut best_distance = f32::MAX;
326            for _iteration in 0..iteration_count {
327                // Tolerance early exit
328                let eff_pos = translation(self.pose.world_matrices()[target_bone.as_usize()]);
329                let ik_pos = translation(self.pose.world_matrices()[ik_bone.as_usize()]);
330                final_distance = (eff_pos - ik_pos).length();
331                if final_distance <= tolerance {
332                    self.ik_stats[ik_index].tolerance_precheck_breaks += 1;
333                    broke_early = true;
334                    break;
335                }
336                self.ik_stats[ik_index].executed_iterations += 1;
337
338                for link_index in 0..link_count {
339                    let link = &links[link_index];
340                    let link_bone = link.bone;
341                    self.ik_stats[ik_index].link_visits += 1;
342
343                    if link_bone == target_bone {
344                        continue;
345                    }
346
347                    let link_world = self.pose.world_matrices()[link_bone.as_usize()];
348                    let link_pos = translation(link_world);
349                    let eff_pos = translation(self.pose.world_matrices()[target_bone.as_usize()]);
350                    let ik_pos = translation(self.pose.world_matrices()[ik_bone.as_usize()]);
351
352                    // Transform direction vectors to link-local space
353                    let link_world_rot = rotation(link_world);
354                    let local_effector = link_world_rot.inverse().mul_vec3a(eff_pos - link_pos);
355                    let local_target = link_world_rot.inverse().mul_vec3a(ik_pos - link_pos);
356
357                    if local_effector.length_squared() <= f32::EPSILON
358                        || local_target.length_squared() <= f32::EPSILON
359                    {
360                        continue;
361                    }
362
363                    solve_link_step(LinkStepInput {
364                        local_effector: &local_effector,
365                        local_target: &local_target,
366                        link_index,
367                        base_rotations: &base_rotations,
368                        ik_rotations: &mut ik_rotations,
369                        chain_states: &mut chain_states,
370                        angle_limit: link.angle_limit,
371                        iteration: _iteration,
372                        limit_angle,
373                    });
374
375                    self.apply_ik_link_rotations(&links, &base_rotations, &ik_rotations);
376                    self.update_world_matrices_from_bone(link_bone);
377                    self.ik_stats[ik_index].link_steps += 1;
378                }
379
380                // Best rotations tracking
381                let current_distance = {
382                    let eff = translation(self.pose.world_matrices()[target_bone.as_usize()]);
383                    let ik = translation(self.pose.world_matrices()[ik_bone.as_usize()]);
384                    (eff - ik).length()
385                };
386                final_distance = current_distance;
387
388                if current_distance < best_distance {
389                    best_distance = current_distance;
390                    best_ik_rotations.copy_from_slice(&ik_rotations);
391                    if current_distance <= tolerance {
392                        self.ik_stats[ik_index].tolerance_post_iteration_breaks += 1;
393                        broke_early = true;
394                        break;
395                    }
396                } else {
397                    self.ik_stats[ik_index].rollback_breaks += 1;
398                    ik_rotations.copy_from_slice(&best_ik_rotations);
399                    self.apply_ik_link_rotations(&links, &base_rotations, &ik_rotations);
400                    if let Some(start_position) = self.min_link_eval_order_position(&links) {
401                        self.update_world_matrices_from_eval_order_position(start_position);
402                    }
403                    broke_early = true;
404                    break;
405                }
406            }
407            self.ik_stats[ik_index].final_distance_sum += f64::from(final_distance);
408            self.ik_stats[ik_index].final_distance_max = self.ik_stats[ik_index]
409                .final_distance_max
410                .max(final_distance);
411            if !broke_early {
412                self.ik_stats[ik_index].max_iteration_exhaustions += 1;
413                self.ik_stats[ik_index].exhausted_final_distance_sum += f64::from(final_distance);
414                self.ik_stats[ik_index].exhausted_final_distance_max = self.ik_stats[ik_index]
415                    .exhausted_final_distance_max
416                    .max(final_distance);
417            }
418
419            // Apply final best effective rotations
420            self.apply_ik_link_rotations(&links, &base_rotations, &best_ik_rotations);
421            if let Some(start_position) = self.min_link_eval_order_position(&links) {
422                self.update_world_matrices_from_eval_order_position(start_position);
423            }
424        } // close for-ik_index
425
426        self.ik_scratch.links = links;
427        self.ik_scratch.base_rotations = base_rotations;
428        self.ik_scratch.ik_rotations = ik_rotations;
429        self.ik_scratch.best_ik_rotations = best_ik_rotations;
430        self.ik_scratch.chain_states = chain_states;
431    }
432
433    fn apply_ik_link_rotations(
434        &mut self,
435        links: &[crate::IkLink],
436        base_rotations: &[Quat],
437        ik_rotations: &[Quat],
438    ) {
439        for (i, link) in links.iter().enumerate() {
440            let effective = (ik_rotations[i] * base_rotations[i]).normalize();
441            self.pose.set_local_rotation(link.bone, effective);
442        }
443    }
444
445    pub fn evaluate_rest_pose(&mut self) {
446        self.pose.reset_local_pose();
447        self.evaluate_current_pose();
448    }
449
450    pub fn evaluate_clip_frame(&mut self, clip: &AnimationClip, frame: f32) {
451        clip.apply_to_pose(frame, &mut self.pose);
452        self.expand_morphs();
453        self.evaluate_current_pose();
454    }
455
456    pub fn evaluate_clip_frame_with_ik_options(
457        &mut self,
458        clip: &AnimationClip,
459        frame: f32,
460        options: IkSolveOptions,
461    ) {
462        clip.apply_to_pose(frame, &mut self.pose);
463        self.expand_morphs();
464        self.evaluate_current_pose_with_ik_options(options);
465    }
466
467    /// Evaluate a clip frame but stop before solving IK. Applies the clip to
468    /// the pose, expands morphs, and updates world matrices - the same setup
469    /// as [`Self::evaluate_clip_frame`] but without calling `solve_enabled_ik`.
470    /// Useful for diagnostics that need to inspect pre-IK runtime state.
471    pub fn evaluate_clip_frame_without_ik(&mut self, clip: &AnimationClip, frame: f32) {
472        clip.apply_to_pose(frame, &mut self.pose);
473        self.expand_morphs();
474        self.update_world_matrices();
475    }
476
477    /// Expand group morphs and apply bone morph offsets.
478    ///
479    /// Called automatically from [`Self::evaluate_clip_frame`]. Exposed publicly so
480    /// that hosts manually driving [`PoseArena`] can trigger morph expansion
481    /// before calling [`Self::evaluate_current_pose`].
482    pub fn expand_morphs(&mut self) {
483        self.expand_group_morphs();
484        self.apply_bone_morphs();
485    }
486
487    /// Pass 1: expand all group morph weights (updates morph_weights in-place).
488    /// Group morph children may appear before or after their parents in PMX, so
489    /// expansion follows the graph recursively using the model-validated
490    /// cycle-free group morph spans.
491    fn expand_group_morphs(&mut self) {
492        let spans = self.model.group_morph_spans();
493        let offsets = self.model.group_morph_offsets();
494        if spans.is_empty() || offsets.is_empty() {
495            return;
496        }
497        let mc = self.model.morph_count() as usize;
498        self.morph_scratch.expanded_weights.clear();
499        self.morph_scratch
500            .expanded_weights
501            .extend_from_slice(&self.pose.morph_weights()[..mc]);
502
503        for (morph_idx, &w) in self.pose.morph_weights()[..mc].iter().enumerate() {
504            if w == 0.0 {
505                continue;
506            }
507            expand_group_morph_weight(
508                morph_idx,
509                w,
510                spans,
511                offsets,
512                &mut self.morph_scratch.expanded_weights,
513            );
514        }
515        for (i, &w) in self.morph_scratch.expanded_weights.iter().enumerate() {
516            self.pose.set_morph_weight(MorphIndex(i as u32), w);
517        }
518    }
519
520    /// Pass 2: apply bone morph offsets using the final (expanded) morph
521    /// weights.
522    fn apply_bone_morphs(&mut self) {
523        let spans = self.model.bone_morph_spans();
524        let offsets = self.model.bone_morph_offsets();
525        if spans.is_empty() || offsets.is_empty() {
526            return;
527        }
528        for (morph_idx, span) in spans.iter().enumerate() {
529            let weight = self.pose.morph_weight(MorphIndex(morph_idx as u32));
530            if weight == 0.0 {
531                continue;
532            }
533            for i in span.start..span.start + span.count {
534                let off = &offsets[i as usize];
535                let pos = self.pose.local_position_offset(off.target_bone);
536                self.pose
537                    .set_local_position_offset(off.target_bone, pos + off.position_offset * weight);
538                let rot = self.pose.local_rotation(off.target_bone);
539                let scaled = Quat::IDENTITY.slerp(off.rotation_offset, weight);
540                self.pose
541                    .set_local_rotation(off.target_bone, (rot * scaled).normalize());
542            }
543        }
544    }
545
546    #[inline]
547    pub fn world_matrices(&self) -> &[Mat4] {
548        self.pose.world_matrices()
549    }
550
551    #[cfg(test)]
552    fn reset_world_matrix_bone_update_count(&mut self) {
553        self.world_matrix_bone_update_count = 0;
554    }
555
556    #[cfg(test)]
557    fn world_matrix_bone_update_count(&self) -> usize {
558        self.world_matrix_bone_update_count
559    }
560
561    #[inline]
562    pub fn skinning_matrices(&self) -> &[Mat4] {
563        self.pose.skinning_matrices()
564    }
565
566    #[inline]
567    pub fn morph_weights(&self) -> &[f32] {
568        self.pose.morph_weights()
569    }
570
571    pub fn reset_ik_runtime_stats(&mut self) {
572        for stats in &mut self.ik_stats {
573            stats.reset();
574        }
575    }
576
577    pub fn ik_runtime_stats(&self) -> &[IkSolverRuntimeStats] {
578        &self.ik_stats
579    }
580
581    #[inline]
582    pub fn ik_enabled(&self) -> &[u8] {
583        self.pose.ik_enabled()
584    }
585}
586
587fn expand_group_morph_weight(
588    morph_idx: usize,
589    weight: f32,
590    spans: &[crate::MorphOffsetSpan],
591    offsets: &[crate::GroupMorphOffset],
592    expanded_weights: &mut [f32],
593) {
594    let span = spans[morph_idx];
595    for i in span.start..span.start + span.count {
596        let off = &offsets[i as usize];
597        let child = off.child_morph.as_usize();
598        let contribution = weight * off.ratio;
599        expanded_weights[child] += contribution;
600        if spans[child].count > 0 {
601            expand_group_morph_weight(child, contribution, spans, offsets, expanded_weights);
602        }
603    }
604}
605
606#[cfg(test)]
607mod tests {
608    use std::sync::Arc;
609
610    use glam::{Quat, Vec3A};
611
612    use crate::{
613        AnimationClip, AppendTransformInit, BoneAnimationBinding, BoneIndex, BoneInit,
614        IkAngleLimit, IkLinkInit, IkSolverInit, ModelArena, MovableBoneKeyframe, MovableBoneTrack,
615        RuntimeInstance,
616    };
617
618    fn translation(matrix: glam::Mat4) -> Vec3A {
619        Vec3A::from_vec4(matrix.w_axis)
620    }
621
622    fn assert_vec3a_near(actual: Vec3A, expected: Vec3A) {
623        let delta = (actual - expected).abs();
624        assert!(
625            delta.x < 1.0e-5 && delta.y < 1.0e-5 && delta.z < 1.0e-5,
626            "actual={actual:?} expected={expected:?} delta={delta:?}"
627        );
628    }
629
630    #[test]
631    fn evaluates_rest_pose_world_matrices() {
632        let model = Arc::new(
633            ModelArena::new(vec![
634                BoneInit::new(None, Vec3A::new(1.0, 0.0, 0.0)),
635                BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 2.0, 0.0)),
636            ])
637            .unwrap(),
638        );
639        let mut runtime = RuntimeInstance::new(model);
640
641        runtime.evaluate_rest_pose();
642
643        assert_vec3a_near(
644            translation(runtime.world_matrices()[0]),
645            Vec3A::new(1.0, 0.0, 0.0),
646        );
647        assert_vec3a_near(
648            translation(runtime.world_matrices()[1]),
649            Vec3A::new(1.0, 2.0, 0.0),
650        );
651    }
652
653    #[test]
654    fn evaluates_current_pose_with_parent_rotation() {
655        let model = Arc::new(
656            ModelArena::new(vec![
657                BoneInit::new(None, Vec3A::new(1.0, 0.0, 0.0)),
658                BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 2.0, 0.0)),
659            ])
660            .unwrap(),
661        );
662        let mut runtime = RuntimeInstance::new(model);
663
664        runtime.pose_mut().set_local_rotation(
665            BoneIndex(0),
666            Quat::from_rotation_z(std::f32::consts::FRAC_PI_2),
667        );
668        runtime.evaluate_current_pose();
669
670        assert_vec3a_near(
671            translation(runtime.world_matrices()[1]),
672            Vec3A::new(-1.0, 0.0, 0.0),
673        );
674    }
675
676    #[test]
677    fn fixed_axis_bone_rotation_keeps_only_axis_twist() {
678        let model = Arc::new(
679            ModelArena::new(vec![
680                BoneInit::new(None, Vec3A::ZERO).with_fixed_axis(Vec3A::Y),
681                BoneInit::new(Some(BoneIndex(0)), Vec3A::X),
682            ])
683            .unwrap(),
684        );
685        let mut runtime = RuntimeInstance::new(model);
686
687        runtime.pose_mut().set_local_rotation(
688            BoneIndex(0),
689            (Quat::from_rotation_y(std::f32::consts::FRAC_PI_2)
690                * Quat::from_rotation_x(std::f32::consts::FRAC_PI_2))
691            .normalize(),
692        );
693        runtime.evaluate_current_pose();
694
695        assert_vec3a_near(
696            translation(runtime.world_matrices()[1]),
697            Vec3A::new(0.0, 0.0, -1.0),
698        );
699    }
700
701    #[test]
702    fn evaluates_current_pose_with_local_position_offset() {
703        let model = Arc::new(
704            ModelArena::new(vec![
705                BoneInit::new(None, Vec3A::new(1.0, 0.0, 0.0)),
706                BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 2.0, 0.0)),
707            ])
708            .unwrap(),
709        );
710        let mut runtime = RuntimeInstance::new(model);
711
712        runtime
713            .pose_mut()
714            .set_local_position_offset(BoneIndex(1), Vec3A::new(0.0, 0.0, 3.0));
715        runtime.evaluate_current_pose();
716
717        assert_vec3a_near(
718            translation(runtime.world_matrices()[1]),
719            Vec3A::new(1.0, 2.0, 3.0),
720        );
721    }
722
723    #[test]
724    fn evaluates_clip_frame_into_world_matrices() {
725        let model = Arc::new(
726            ModelArena::new(vec![
727                BoneInit::new(None, Vec3A::new(1.0, 0.0, 0.0)),
728                BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 2.0, 0.0)),
729            ])
730            .unwrap(),
731        );
732        let clip = AnimationClip::new(vec![BoneAnimationBinding {
733            bone: BoneIndex(1),
734            track: MovableBoneTrack::from_keyframes(vec![
735                MovableBoneKeyframe::new(0, Vec3A::ZERO, Quat::IDENTITY),
736                MovableBoneKeyframe::new(10, Vec3A::new(0.0, 0.0, 4.0), Quat::IDENTITY),
737            ]),
738        }]);
739        let mut runtime = RuntimeInstance::new(model);
740
741        runtime.evaluate_clip_frame(&clip, 5.0);
742
743        assert_vec3a_near(
744            translation(runtime.world_matrices()[1]),
745            Vec3A::new(1.0, 2.0, 2.0),
746        );
747    }
748
749    #[test]
750    fn applies_append_rotation_before_world_matrix_output() {
751        let model = Arc::new(
752            ModelArena::new_full(
753                vec![
754                    BoneInit::new(None, Vec3A::ZERO),
755                    BoneInit::new(None, Vec3A::ZERO),
756                    BoneInit::new(Some(BoneIndex(1)), Vec3A::new(1.0, 0.0, 0.0)),
757                ],
758                Vec::new(),
759                vec![AppendTransformInit::new(BoneIndex(1), BoneIndex(0), 1.0).with_rotation()],
760            )
761            .unwrap(),
762        );
763        let mut runtime = RuntimeInstance::new(model);
764
765        runtime.pose_mut().set_local_rotation(
766            BoneIndex(0),
767            Quat::from_rotation_z(std::f32::consts::FRAC_PI_2),
768        );
769        runtime.evaluate_current_pose();
770
771        assert_vec3a_near(
772            translation(runtime.world_matrices()[2]),
773            Vec3A::new(0.0, 1.0, 0.0),
774        );
775    }
776
777    #[test]
778    fn applies_append_translation_before_world_matrix_output() {
779        let model = Arc::new(
780            ModelArena::new_full(
781                vec![
782                    BoneInit::new(None, Vec3A::ZERO),
783                    BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
784                ],
785                Vec::new(),
786                vec![AppendTransformInit::new(BoneIndex(1), BoneIndex(0), 0.5).with_translation()],
787            )
788            .unwrap(),
789        );
790        let mut runtime = RuntimeInstance::new(model);
791
792        runtime
793            .pose_mut()
794            .set_local_position_offset(BoneIndex(0), Vec3A::new(2.0, 0.0, 0.0));
795        runtime.evaluate_current_pose();
796
797        assert_vec3a_near(
798            translation(runtime.world_matrices()[1]),
799            Vec3A::new(1.0, 1.0, 0.0),
800        );
801    }
802
803    #[test]
804    fn initializes_ik_enabled_from_model_solvers() {
805        let model = Arc::new(
806            ModelArena::new_with_ik(
807                vec![
808                    BoneInit::new(None, Vec3A::ZERO),
809                    BoneInit::new(Some(BoneIndex(0)), Vec3A::ZERO),
810                ],
811                vec![IkSolverInit::new(
812                    BoneIndex(1),
813                    BoneIndex(0),
814                    vec![IkLinkInit::new(BoneIndex(0))],
815                )],
816            )
817            .unwrap(),
818        );
819
820        let runtime = RuntimeInstance::new(model);
821
822        assert_eq!(runtime.ik_enabled(), &[1]);
823    }
824
825    #[test]
826    fn solves_one_link_ik_toward_controller_bone() {
827        let model = Arc::new(
828            ModelArena::new_with_ik(
829                vec![
830                    BoneInit::new(None, Vec3A::ZERO),
831                    BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
832                    BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
833                ],
834                vec![IkSolverInit {
835                    ik_bone: BoneIndex(2),
836                    target_bone: BoneIndex(1),
837                    links: vec![IkLinkInit::new(BoneIndex(0))],
838                    iteration_count: 1,
839                    limit_angle: 0.0,
840                }],
841            )
842            .unwrap(),
843        );
844        let mut runtime = RuntimeInstance::new(model);
845
846        runtime.evaluate_current_pose();
847
848        assert_vec3a_near(
849            translation(runtime.world_matrices()[1]),
850            Vec3A::new(0.0, 1.0, 0.0),
851        );
852    }
853
854    #[test]
855    fn skips_disabled_ik_solver() {
856        let model = Arc::new(
857            ModelArena::new_with_ik(
858                vec![
859                    BoneInit::new(None, Vec3A::ZERO),
860                    BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
861                    BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
862                ],
863                vec![IkSolverInit {
864                    ik_bone: BoneIndex(2),
865                    target_bone: BoneIndex(1),
866                    links: vec![IkLinkInit::new(BoneIndex(0))],
867                    iteration_count: 1,
868                    limit_angle: 0.0,
869                }],
870            )
871            .unwrap(),
872        );
873        let mut runtime = RuntimeInstance::new(model);
874
875        runtime.pose_mut().set_ik_enabled(0, false);
876        runtime.evaluate_current_pose();
877
878        assert_vec3a_near(
879            translation(runtime.world_matrices()[1]),
880            Vec3A::new(1.0, 0.0, 0.0),
881        );
882    }
883
884    #[test]
885    fn solves_two_link_ik_chain_toward_controller_bone() {
886        let model = Arc::new(
887            ModelArena::new_with_ik(
888                vec![
889                    BoneInit::new(None, Vec3A::ZERO),
890                    BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
891                    BoneInit::new(Some(BoneIndex(1)), Vec3A::new(1.0, 0.0, 0.0)),
892                    BoneInit::new(None, Vec3A::new(1.0, 1.0, 0.0)),
893                ],
894                vec![IkSolverInit {
895                    ik_bone: BoneIndex(3),
896                    target_bone: BoneIndex(2),
897                    links: vec![IkLinkInit::new(BoneIndex(1)), IkLinkInit::new(BoneIndex(0))],
898                    iteration_count: 4,
899                    limit_angle: 0.0,
900                }],
901            )
902            .unwrap(),
903        );
904        let mut runtime = RuntimeInstance::new(model);
905
906        runtime.evaluate_current_pose();
907
908        assert_vec3a_near(
909            translation(runtime.world_matrices()[2]),
910            Vec3A::new(1.0, 1.0, 0.0),
911        );
912    }
913
914    #[test]
915    fn ik_updates_only_affected_eval_suffix_for_late_chain() {
916        let unrelated_count = 96usize;
917        let chain_root = BoneIndex(unrelated_count as u32);
918        let chain_mid = BoneIndex(unrelated_count as u32 + 1);
919        let chain_tip = BoneIndex(unrelated_count as u32 + 2);
920        let controller = BoneIndex(unrelated_count as u32 + 3);
921
922        let mut bones = Vec::new();
923        for i in 0..unrelated_count {
924            bones.push(BoneInit::new(None, Vec3A::new(i as f32 * 10.0, -10.0, 0.0)));
925        }
926        bones.push(BoneInit::new(None, Vec3A::ZERO));
927        bones.push(BoneInit::new(Some(chain_root), Vec3A::new(1.0, 0.0, 0.0)));
928        bones.push(BoneInit::new(Some(chain_mid), Vec3A::new(1.0, 0.0, 0.0)));
929        bones.push(BoneInit::new(None, Vec3A::new(1.0, 1.0, 0.0)));
930
931        let model = Arc::new(
932            ModelArena::new_with_ik(
933                bones,
934                vec![IkSolverInit {
935                    ik_bone: controller,
936                    target_bone: chain_tip,
937                    links: vec![IkLinkInit::new(chain_mid), IkLinkInit::new(chain_root)],
938                    iteration_count: 4,
939                    limit_angle: 0.0,
940                }],
941            )
942            .unwrap(),
943        );
944        let mut runtime = RuntimeInstance::new(model);
945
946        runtime.reset_world_matrix_bone_update_count();
947        runtime.evaluate_current_pose();
948
949        assert_vec3a_near(
950            translation(runtime.world_matrices()[chain_tip.as_usize()]),
951            Vec3A::new(1.0, 1.0, 0.0),
952        );
953        assert!(
954            runtime.world_matrix_bone_update_count() < 250,
955            "IK should not recompute unrelated prefix bones repeatedly; updated {} bones",
956            runtime.world_matrix_bone_update_count()
957        );
958    }
959
960    #[test]
961    fn clamps_ik_rotation_by_solver_limit_angle() {
962        let model = Arc::new(
963            ModelArena::new_with_ik(
964                vec![
965                    BoneInit::new(None, Vec3A::ZERO),
966                    BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
967                    BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
968                ],
969                vec![IkSolverInit {
970                    ik_bone: BoneIndex(2),
971                    target_bone: BoneIndex(1),
972                    links: vec![IkLinkInit::new(BoneIndex(0))],
973                    iteration_count: 1,
974                    limit_angle: std::f32::consts::FRAC_PI_4,
975                }],
976            )
977            .unwrap(),
978        );
979        let mut runtime = RuntimeInstance::new(model);
980
981        runtime.evaluate_current_pose();
982
983        let expected = Vec3A::new(
984            std::f32::consts::FRAC_1_SQRT_2,
985            std::f32::consts::FRAC_1_SQRT_2,
986            0.0,
987        );
988        assert_vec3a_near(translation(runtime.world_matrices()[1]), expected);
989    }
990
991    #[test]
992    fn applies_constant_limit_angle_per_iteration() {
993        let model = Arc::new(
994            ModelArena::new_with_ik(
995                vec![
996                    BoneInit::new(None, Vec3A::ZERO),
997                    BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
998                    BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
999                ],
1000                vec![IkSolverInit {
1001                    ik_bone: BoneIndex(2),
1002                    target_bone: BoneIndex(1),
1003                    links: vec![IkLinkInit::new(BoneIndex(1)), IkLinkInit::new(BoneIndex(0))],
1004                    iteration_count: 1,
1005                    limit_angle: std::f32::consts::FRAC_PI_4,
1006                }],
1007            )
1008            .unwrap(),
1009        );
1010        let mut runtime = RuntimeInstance::new(model);
1011
1012        runtime.evaluate_current_pose();
1013
1014        // With constant limit_angle = PI/4 (not scaled by link_index), only the root
1015        // (link 1, bone 0) rotates at most PI/4. The effector bone is skipped.
1016        // The child bone ends up at (cos(PI/4)*1, sin(PI/4)*1, 0)
1017        let expected = Vec3A::new(
1018            std::f32::consts::FRAC_1_SQRT_2,
1019            std::f32::consts::FRAC_1_SQRT_2,
1020            0.0,
1021        );
1022        assert_vec3a_near(translation(runtime.world_matrices()[1]), expected);
1023    }
1024
1025    #[test]
1026    fn clip_frame_produces_deterministic_world_translations() {
1027        let model = Arc::new(
1028            ModelArena::new(vec![
1029                BoneInit::new(None, Vec3A::new(1.0, 0.0, 0.0)),
1030                BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 2.0, 0.0)),
1031            ])
1032            .unwrap(),
1033        );
1034        let clip = AnimationClip::new(vec![BoneAnimationBinding {
1035            bone: BoneIndex(1),
1036            track: MovableBoneTrack::from_keyframes(vec![
1037                MovableBoneKeyframe::new(0, Vec3A::ZERO, Quat::IDENTITY),
1038                MovableBoneKeyframe::new(10, Vec3A::new(0.0, 0.0, 4.0), Quat::IDENTITY),
1039            ]),
1040        }]);
1041        let mut runtime = RuntimeInstance::new(model);
1042
1043        runtime.evaluate_clip_frame(&clip, 5.0);
1044
1045        let matrices = runtime.world_matrices();
1046        assert_eq!(matrices.len(), 2);
1047        assert_vec3a_near(translation(matrices[0]), Vec3A::new(1.0, 0.0, 0.0));
1048        assert_vec3a_near(translation(matrices[1]), Vec3A::new(1.0, 2.0, 2.0));
1049    }
1050
1051    #[test]
1052    fn evaluate_clip_frame_without_ik_leaves_ik_unsolved() {
1053        let model = Arc::new(
1054            ModelArena::new_with_ik(
1055                vec![
1056                    BoneInit::new(None, Vec3A::ZERO),
1057                    BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1058                    BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1059                ],
1060                vec![IkSolverInit {
1061                    ik_bone: BoneIndex(2),
1062                    target_bone: BoneIndex(1),
1063                    links: vec![IkLinkInit::new(BoneIndex(0))],
1064                    iteration_count: 1,
1065                    limit_angle: 0.0,
1066                }],
1067            )
1068            .unwrap(),
1069        );
1070        let clip = AnimationClip::new(vec![]);
1071
1072        let mut without_ik = RuntimeInstance::new(Arc::clone(&model));
1073        let mut with_ik = RuntimeInstance::new(model);
1074
1075        without_ik.evaluate_clip_frame_without_ik(&clip, 0.0);
1076        with_ik.evaluate_clip_frame(&clip, 0.0);
1077
1078        // Without IK: effector bone stays at rest position (1, 0, 0)
1079        assert_vec3a_near(
1080            translation(without_ik.world_matrices()[1]),
1081            Vec3A::new(1.0, 0.0, 0.0),
1082        );
1083        // With IK: effector bone rotates toward target at (0, 1, 0)
1084        assert_vec3a_near(
1085            translation(with_ik.world_matrices()[1]),
1086            Vec3A::new(0.0, 1.0, 0.0),
1087        );
1088    }
1089
1090    #[test]
1091    fn ik_options_cap_configured_iterations() {
1092        let model = Arc::new(
1093            ModelArena::new_with_ik(
1094                vec![
1095                    BoneInit::new(None, Vec3A::ZERO),
1096                    BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1097                    BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1098                ],
1099                vec![IkSolverInit {
1100                    ik_bone: BoneIndex(2),
1101                    target_bone: BoneIndex(1),
1102                    links: vec![IkLinkInit::new(BoneIndex(0))],
1103                    iteration_count: 100,
1104                    limit_angle: 0.0,
1105                }],
1106            )
1107            .unwrap(),
1108        );
1109        let mut runtime = RuntimeInstance::new(model);
1110
1111        runtime.reset_ik_runtime_stats();
1112        runtime.evaluate_current_pose_with_ik_options(super::IkSolveOptions {
1113            tolerance: 0.0,
1114            max_iterations_cap: Some(5),
1115        });
1116
1117        assert_eq!(runtime.ik_runtime_stats()[0].configured_iterations, 5);
1118    }
1119
1120    // ---- morph expansion tests ----
1121
1122    fn assert_near(actual: f32, expected: f32) {
1123        let delta = (actual - expected).abs();
1124        assert!(
1125            delta < 1.0e-5,
1126            "actual={actual:?} expected={expected:?} delta={delta:?}"
1127        );
1128    }
1129
1130    #[test]
1131    fn bone_morph_position_offset_drives_world_position() {
1132        let model = Arc::new(
1133            ModelArena::new_with_morphs(
1134                vec![
1135                    BoneInit::new(None, Vec3A::ZERO),
1136                    BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 1.0, 0.0)),
1137                ],
1138                Vec::new(),
1139                Vec::new(),
1140                crate::MorphInit {
1141                    morph_count: 1,
1142                    bone_offsets: vec![crate::BoneMorphOffset {
1143                        target_bone: BoneIndex(1),
1144                        position_offset: Vec3A::new(0.0, 0.0, 2.0),
1145                        rotation_offset: Quat::IDENTITY,
1146                    }],
1147                    bone_spans: vec![crate::MorphOffsetSpan { start: 0, count: 1 }],
1148                    group_offsets: vec![],
1149                    group_spans: vec![crate::MorphOffsetSpan::default()],
1150                    ..crate::MorphInit::default()
1151                },
1152            )
1153            .unwrap(),
1154        );
1155        let clip = AnimationClip::new_with_morphs(
1156            Vec::new(),
1157            vec![crate::MorphAnimationBinding {
1158                morph: crate::MorphIndex(0),
1159                track: crate::MorphTrack::from_keyframes(vec![
1160                    crate::MorphKeyframe::new(0, 0.0),
1161                    crate::MorphKeyframe::new(10, 1.0),
1162                ]),
1163            }],
1164        );
1165        let mut runtime = RuntimeInstance::new_with_morph_count(model, 1);
1166
1167        runtime.evaluate_clip_frame(&clip, 5.0);
1168
1169        // weight = 0.5: bone offset = (0,0,2) * 0.5 = (0,0,1)
1170        assert_vec3a_near(
1171            translation(runtime.world_matrices()[1]),
1172            Vec3A::new(0.0, 1.0, 1.0),
1173        );
1174        assert_near(runtime.morph_weights()[0], 0.5);
1175    }
1176
1177    #[test]
1178    fn bone_morph_rotation_offset_affects_child_position() {
1179        let model = Arc::new(
1180            ModelArena::new_with_morphs(
1181                vec![
1182                    BoneInit::new(None, Vec3A::ZERO),
1183                    BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1184                    BoneInit::new(Some(BoneIndex(1)), Vec3A::new(1.0, 0.0, 0.0)),
1185                ],
1186                Vec::new(),
1187                Vec::new(),
1188                crate::MorphInit {
1189                    morph_count: 1,
1190                    bone_offsets: vec![crate::BoneMorphOffset {
1191                        target_bone: BoneIndex(1),
1192                        position_offset: Vec3A::ZERO,
1193                        rotation_offset: Quat::from_rotation_z(std::f32::consts::FRAC_PI_2),
1194                    }],
1195                    bone_spans: vec![crate::MorphOffsetSpan { start: 0, count: 1 }],
1196                    group_offsets: vec![],
1197                    group_spans: vec![crate::MorphOffsetSpan::default()],
1198                    ..crate::MorphInit::default()
1199                },
1200            )
1201            .unwrap(),
1202        );
1203        let clip = AnimationClip::new_with_morphs(
1204            Vec::new(),
1205            vec![crate::MorphAnimationBinding {
1206                morph: crate::MorphIndex(0),
1207                track: crate::MorphTrack::from_keyframes(vec![
1208                    crate::MorphKeyframe::new(0, 0.0),
1209                    crate::MorphKeyframe::new(10, 1.0),
1210                ]),
1211            }],
1212        );
1213        let mut runtime = RuntimeInstance::new_with_morph_count(model, 1);
1214
1215        runtime.evaluate_clip_frame(&clip, 10.0);
1216
1217        // weight = 1.0: bone 1 (rest 1,0,0) rotated Z-90 by morph (position unchanged)
1218        // bone 2 at (1,0,0) relative to bone 1: world = (1,0,0) + (0,1,0)
1219        assert_vec3a_near(
1220            translation(runtime.world_matrices()[2]),
1221            Vec3A::new(1.0, 1.0, 0.0),
1222        );
1223    }
1224
1225    #[test]
1226    fn group_morph_contributes_to_bone_morph_weight() {
1227        // PMX order: child (bone morph) has smaller index than parent (group morph)
1228        // Morph 0 = bone morph, Morph 1 = group morph with MorphIndex(0) as child.
1229        let model = Arc::new(
1230            ModelArena::new_with_morphs(
1231                vec![
1232                    BoneInit::new(None, Vec3A::ZERO),
1233                    BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 1.0, 0.0)),
1234                ],
1235                Vec::new(),
1236                Vec::new(),
1237                crate::MorphInit {
1238                    morph_count: 2,
1239                    bone_offsets: vec![crate::BoneMorphOffset {
1240                        target_bone: BoneIndex(1),
1241                        position_offset: Vec3A::new(0.0, 0.0, 2.0),
1242                        rotation_offset: Quat::IDENTITY,
1243                    }],
1244                    bone_spans: vec![
1245                        crate::MorphOffsetSpan { start: 0, count: 1 },
1246                        crate::MorphOffsetSpan::default(),
1247                    ],
1248                    group_offsets: vec![crate::GroupMorphOffset {
1249                        child_morph: crate::MorphIndex(0),
1250                        ratio: 0.5,
1251                    }],
1252                    group_spans: vec![
1253                        crate::MorphOffsetSpan::default(),
1254                        crate::MorphOffsetSpan { start: 0, count: 1 },
1255                    ],
1256                    ..crate::MorphInit::default()
1257                },
1258            )
1259            .unwrap(),
1260        );
1261        // VMD track only on group morph (index 1), weight = 1.0
1262        let clip = AnimationClip::new_with_morphs(
1263            Vec::new(),
1264            vec![crate::MorphAnimationBinding {
1265                morph: crate::MorphIndex(1),
1266                track: crate::MorphTrack::from_keyframes(vec![
1267                    crate::MorphKeyframe::new(0, 0.0),
1268                    crate::MorphKeyframe::new(10, 1.0),
1269                ]),
1270            }],
1271        );
1272        let mut runtime = RuntimeInstance::new_with_morph_count(model, 2);
1273
1274        runtime.evaluate_clip_frame(&clip, 10.0);
1275
1276        // Group expansion: morph_weights[0] += 1.0 * 0.5 = 0.5
1277        // Bone morph applies: (0,0,2) * 0.5 = (0,0,1)
1278        assert_near(runtime.morph_weights()[0], 0.5);
1279        assert_near(runtime.morph_weights()[1], 1.0);
1280        assert_vec3a_near(
1281            translation(runtime.world_matrices()[1]),
1282            Vec3A::new(0.0, 1.0, 1.0),
1283        );
1284    }
1285
1286    #[test]
1287    fn group_morph_can_reference_later_child_morph() {
1288        let model = Arc::new(
1289            ModelArena::new_with_morphs(
1290                vec![
1291                    BoneInit::new(None, Vec3A::ZERO),
1292                    BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 1.0, 0.0)),
1293                ],
1294                Vec::new(),
1295                Vec::new(),
1296                crate::MorphInit {
1297                    morph_count: 2,
1298                    bone_offsets: vec![crate::BoneMorphOffset {
1299                        target_bone: BoneIndex(1),
1300                        position_offset: Vec3A::new(0.0, 0.0, 2.0),
1301                        rotation_offset: Quat::IDENTITY,
1302                    }],
1303                    bone_spans: vec![
1304                        crate::MorphOffsetSpan::default(),
1305                        crate::MorphOffsetSpan { start: 0, count: 1 },
1306                    ],
1307                    group_offsets: vec![crate::GroupMorphOffset {
1308                        child_morph: crate::MorphIndex(1),
1309                        ratio: 0.5,
1310                    }],
1311                    group_spans: vec![
1312                        crate::MorphOffsetSpan { start: 0, count: 1 },
1313                        crate::MorphOffsetSpan::default(),
1314                    ],
1315                    ..crate::MorphInit::default()
1316                },
1317            )
1318            .unwrap(),
1319        );
1320        let clip = AnimationClip::new_with_morphs(
1321            Vec::new(),
1322            vec![crate::MorphAnimationBinding {
1323                morph: crate::MorphIndex(0),
1324                track: crate::MorphTrack::from_keyframes(vec![crate::MorphKeyframe::new(0, 1.0)]),
1325            }],
1326        );
1327        let mut runtime = RuntimeInstance::new(model);
1328
1329        runtime.evaluate_clip_frame(&clip, 0.0);
1330
1331        assert_near(runtime.morph_weights()[0], 1.0);
1332        assert_near(runtime.morph_weights()[1], 0.5);
1333        assert_vec3a_near(
1334            translation(runtime.world_matrices()[1]),
1335            Vec3A::new(0.0, 1.0, 1.0),
1336        );
1337    }
1338
1339    #[test]
1340    fn chained_group_morphs_descend_to_bone_morph_weight() {
1341        let model = Arc::new(
1342            ModelArena::new_with_morphs(
1343                vec![
1344                    BoneInit::new(None, Vec3A::ZERO),
1345                    BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 1.0, 0.0)),
1346                ],
1347                Vec::new(),
1348                Vec::new(),
1349                crate::MorphInit {
1350                    morph_count: 3,
1351                    bone_offsets: vec![crate::BoneMorphOffset {
1352                        target_bone: BoneIndex(1),
1353                        position_offset: Vec3A::new(0.0, 0.0, 2.0),
1354                        rotation_offset: Quat::IDENTITY,
1355                    }],
1356                    bone_spans: vec![
1357                        crate::MorphOffsetSpan { start: 0, count: 1 },
1358                        crate::MorphOffsetSpan::default(),
1359                        crate::MorphOffsetSpan::default(),
1360                    ],
1361                    group_offsets: vec![
1362                        crate::GroupMorphOffset {
1363                            child_morph: crate::MorphIndex(0),
1364                            ratio: 0.25,
1365                        },
1366                        crate::GroupMorphOffset {
1367                            child_morph: crate::MorphIndex(1),
1368                            ratio: 0.5,
1369                        },
1370                    ],
1371                    group_spans: vec![
1372                        crate::MorphOffsetSpan::default(),
1373                        crate::MorphOffsetSpan { start: 0, count: 1 },
1374                        crate::MorphOffsetSpan { start: 1, count: 1 },
1375                    ],
1376                    ..crate::MorphInit::default()
1377                },
1378            )
1379            .unwrap(),
1380        );
1381        let clip = AnimationClip::new_with_morphs(
1382            Vec::new(),
1383            vec![crate::MorphAnimationBinding {
1384                morph: crate::MorphIndex(2),
1385                track: crate::MorphTrack::from_keyframes(vec![crate::MorphKeyframe::new(0, 1.0)]),
1386            }],
1387        );
1388        let mut runtime = RuntimeInstance::new(model);
1389
1390        runtime.evaluate_clip_frame(&clip, 0.0);
1391
1392        assert_near(runtime.morph_weights()[2], 1.0);
1393        assert_near(runtime.morph_weights()[1], 0.5);
1394        assert_near(runtime.morph_weights()[0], 0.125);
1395        assert_vec3a_near(
1396            translation(runtime.world_matrices()[1]),
1397            Vec3A::new(0.0, 1.0, 0.25),
1398        );
1399    }
1400
1401    #[test]
1402    fn expand_morphs_noop_when_no_morph_defs() {
1403        let model = Arc::new(ModelArena::new(vec![BoneInit::new(None, Vec3A::ZERO)]).unwrap());
1404        let mut runtime = RuntimeInstance::new_with_morph_count(model, 1);
1405        runtime
1406            .pose_mut()
1407            .set_morph_weight(crate::MorphIndex(0), 1.0);
1408        runtime.expand_morphs();
1409        // No crash = pass
1410        assert_near(runtime.morph_weights()[0], 1.0);
1411    }
1412
1413    #[test]
1414    fn clamps_link_local_rotation_to_angle_limit() {
1415        let model = Arc::new(
1416            ModelArena::new_with_ik(
1417                vec![
1418                    BoneInit::new(None, Vec3A::ZERO),
1419                    BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1420                    BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1421                ],
1422                vec![IkSolverInit {
1423                    ik_bone: BoneIndex(2),
1424                    target_bone: BoneIndex(1),
1425                    links: vec![
1426                        IkLinkInit::new(BoneIndex(0)).with_angle_limit(IkAngleLimit::new(
1427                            Vec3A::new(0.0, 0.0, 0.0),
1428                            Vec3A::new(0.0, 0.0, std::f32::consts::FRAC_PI_4),
1429                        )),
1430                    ],
1431                    iteration_count: 1,
1432                    limit_angle: 0.0,
1433                }],
1434            )
1435            .unwrap(),
1436        );
1437        let mut runtime = RuntimeInstance::new(model);
1438
1439        runtime.evaluate_current_pose();
1440
1441        let expected = Vec3A::new(
1442            std::f32::consts::FRAC_1_SQRT_2,
1443            std::f32::consts::FRAC_1_SQRT_2,
1444            0.0,
1445        );
1446        assert_vec3a_near(translation(runtime.world_matrices()[1]), expected);
1447    }
1448
1449    #[test]
1450    fn multi_axis_limited_link_solves_before_clamping() {
1451        let local_effector = Vec3A::X;
1452        let local_target = Vec3A::new(0.25, 0.55, 0.80).normalize();
1453        let limits = IkAngleLimit::new(Vec3A::new(0.0, -1.0, -1.0), Vec3A::new(0.0, 1.0, 1.0));
1454        let base_rotations = vec![Quat::IDENTITY];
1455        let mut ik_rotations = vec![Quat::IDENTITY];
1456        let mut chain_states = vec![super::ChainLinkState {
1457            previous_euler: [0.0; 3],
1458            plane_mode_angle: 0.0,
1459        }];
1460
1461        super::solve_limited_axes_link_step(super::LimitedAxesLinkStepInput {
1462            local_effector: &local_effector,
1463            local_target: &local_target,
1464            link_index: 0,
1465            base_rotations: &base_rotations,
1466            ik_rotations: &mut ik_rotations,
1467            chain_states: &mut chain_states,
1468            limits,
1469            limit_angle: 0.0,
1470        });
1471
1472        let current_direction = ik_rotations[0].mul_vec3a(local_effector).normalize();
1473        let legacy_direction =
1474            legacy_clamp_only_limited_direction(local_effector, local_target, limits);
1475        let current_error = (current_direction - local_target).length();
1476        let legacy_error = (legacy_direction - local_target).length();
1477
1478        assert!(
1479            current_error < legacy_error - 0.015,
1480            "current_error={current_error:.6} legacy_error={legacy_error:.6} current={current_direction:?} legacy={legacy_direction:?} target={local_target:?}"
1481        );
1482        assert!(
1483            chain_states[0].previous_euler[1].abs() > 0.1
1484                && chain_states[0].previous_euler[2].abs() > 0.1,
1485            "multi-axis limited IK should use both Y and Z axes; euler={:?}",
1486            chain_states[0].previous_euler
1487        );
1488    }
1489
1490    #[test]
1491    fn multi_axis_limited_link_applies_limits_to_total_rotation() {
1492        let local_effector = Vec3A::new(0.25, 0.45, 0.85).normalize();
1493        let local_target = Vec3A::new(0.55, 0.15, 0.80).normalize();
1494        let limits = IkAngleLimit::new(Vec3A::new(-1.0, -1.0, 0.0), Vec3A::new(1.0, 1.0, 0.0));
1495        let base_rotations = vec![Quat::from_rotation_z(0.45)];
1496        let mut ik_rotations = vec![Quat::IDENTITY];
1497        let mut chain_states = vec![super::ChainLinkState {
1498            previous_euler: [0.0; 3],
1499            plane_mode_angle: 0.0,
1500        }];
1501
1502        super::solve_limited_axes_link_step(super::LimitedAxesLinkStepInput {
1503            local_effector: &local_effector,
1504            local_target: &local_target,
1505            link_index: 0,
1506            base_rotations: &base_rotations,
1507            ik_rotations: &mut ik_rotations,
1508            chain_states: &mut chain_states,
1509            limits,
1510            limit_angle: 0.0,
1511        });
1512
1513        let base_direction = base_rotations[0].mul_vec3a(local_effector).normalize();
1514        let effective = (ik_rotations[0] * base_rotations[0]).normalize();
1515        let stale_direction = limited_direction_without_fixed_axis_working_update(
1516            local_effector,
1517            local_target,
1518            base_rotations[0],
1519            limits,
1520        );
1521        let solved_direction = effective.mul_vec3a(local_effector).normalize();
1522        assert_near(chain_states[0].previous_euler[2], 0.0);
1523        assert!(
1524            (solved_direction - stale_direction).length() > 0.05,
1525            "fixed axis clamp should affect later axis solve; solved={solved_direction:?} stale={stale_direction:?}"
1526        );
1527        assert!(
1528            (solved_direction - local_target).length() < (base_direction - local_target).length(),
1529            "non-identity base should still solve toward target; base={base_direction:?} solved={solved_direction:?} target={local_target:?}"
1530        );
1531    }
1532
1533    fn limited_direction_without_fixed_axis_working_update(
1534        local_effector: Vec3A,
1535        local_target: Vec3A,
1536        base: Quat,
1537        limits: IkAngleLimit,
1538    ) -> Vec3A {
1539        let mut total_euler =
1540            super::decompose_euler_xyz(&super::quat_to_rotation_mat3(base), &[0.0; 3]);
1541        let mut working_effector = local_effector;
1542        let target = local_target.normalize();
1543
1544        for axis_index in [2usize, 1, 0] {
1545            let (lower, upper) = super::limit_axis_bounds(limits, axis_index);
1546            if lower == 0.0 && upper == 0.0 {
1547                total_euler[axis_index] = total_euler[axis_index].clamp(lower, upper);
1548                continue;
1549            }
1550
1551            let axis = super::axis_vec(axis_index);
1552            let signed_angle = super::signed_projected_angle(working_effector, target, axis);
1553            if signed_angle.abs() <= 1.0e-6 {
1554                continue;
1555            }
1556            let next = (total_euler[axis_index] + signed_angle).clamp(lower, upper);
1557            let applied = next - total_euler[axis_index];
1558            total_euler[axis_index] = next;
1559            if applied.abs() > 0.0 {
1560                working_effector =
1561                    Quat::from_axis_angle(axis.into(), applied).mul_vec3a(working_effector);
1562            }
1563        }
1564
1565        super::euler_xyz_to_quat(&total_euler)
1566            .normalize()
1567            .mul_vec3a(local_effector)
1568            .normalize()
1569    }
1570
1571    fn legacy_clamp_only_limited_direction(
1572        local_effector: Vec3A,
1573        local_target: Vec3A,
1574        limits: IkAngleLimit,
1575    ) -> Vec3A {
1576        let local_eff_n = local_effector.normalize();
1577        let local_tgt_n = local_target.normalize();
1578        let dot = local_eff_n.dot(local_tgt_n).clamp(-1.0, 1.0);
1579        let angle = dot.acos();
1580        let axis = local_eff_n.cross(local_tgt_n);
1581        let axis_vec = if axis.length() < 1e-5 {
1582            if dot > -1.0 + 1e-5 {
1583                return local_eff_n;
1584            }
1585            let basis = if local_eff_n.x.abs() < 0.9 {
1586                Vec3A::new(1.0, 0.0, 0.0)
1587            } else {
1588                Vec3A::new(0.0, 1.0, 0.0)
1589            };
1590            local_eff_n.cross(basis).normalize()
1591        } else {
1592            axis.normalize()
1593        };
1594        let rotation = Quat::from_axis_angle(axis_vec.into(), angle).normalize();
1595        let euler = super::decompose_euler_xyz(&super::quat_to_rotation_mat3(rotation), &[0.0; 3]);
1596        let clamped = [
1597            euler[0].clamp(limits.min.x, limits.max.x),
1598            euler[1].clamp(limits.min.y, limits.max.y),
1599            euler[2].clamp(limits.min.z, limits.max.z),
1600        ];
1601        super::euler_xyz_to_quat(&clamped)
1602            .normalize()
1603            .mul_vec3a(local_effector)
1604            .normalize()
1605    }
1606
1607    #[test]
1608    fn plane_link_step_matches_saba_total_axis_rotation() {
1609        let base = Quat::from_rotation_x(0.3);
1610        let base_rotations = vec![base];
1611        let mut ik_rotations = vec![Quat::IDENTITY];
1612        let mut chain_states = vec![super::ChainLinkState {
1613            previous_euler: [0.0; 3],
1614            plane_mode_angle: 0.0,
1615        }];
1616        let local_effector = Vec3A::X;
1617        let local_target = Vec3A::Y;
1618
1619        super::solve_plane_link_step(super::PlaneLinkStepInput {
1620            local_effector: &local_effector,
1621            local_target: &local_target,
1622            link_index: 0,
1623            base_rotations: &base_rotations,
1624            ik_rotations: &mut ik_rotations,
1625            chain_states: &mut chain_states,
1626            axis_index: 2,
1627            limits: IkAngleLimit::new(
1628                Vec3A::new(-std::f32::consts::PI, 0.0, -std::f32::consts::PI),
1629                Vec3A::new(std::f32::consts::PI, 0.0, std::f32::consts::PI),
1630            ),
1631            iteration: 0,
1632            limit_angle: 0.0,
1633        });
1634
1635        let effective = (ik_rotations[0] * base_rotations[0]).normalize();
1636        assert_near(
1637            chain_states[0].plane_mode_angle,
1638            std::f32::consts::FRAC_PI_2,
1639        );
1640        assert_vec3a_near(
1641            effective.mul_vec3a(Vec3A::X),
1642            Quat::from_rotation_z(std::f32::consts::FRAC_PI_2).mul_vec3a(Vec3A::X),
1643        );
1644        assert_vec3a_near(effective.mul_vec3a(Vec3A::Z), Vec3A::Z);
1645    }
1646
1647    #[test]
1648    fn append_rotation_propagates_post_ik_link_rotation() {
1649        let model = Arc::new(
1650            ModelArena::new_full(
1651                vec![
1652                    BoneInit::new(None, Vec3A::ZERO),
1653                    BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1654                    BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1655                    BoneInit::new(None, Vec3A::ZERO),
1656                    BoneInit::new(Some(BoneIndex(3)), Vec3A::new(1.0, 0.0, 0.0)),
1657                ],
1658                vec![IkSolverInit {
1659                    ik_bone: BoneIndex(2),
1660                    target_bone: BoneIndex(1),
1661                    links: vec![IkLinkInit::new(BoneIndex(0))],
1662                    iteration_count: 1,
1663                    limit_angle: 0.0,
1664                }],
1665                vec![AppendTransformInit::new(BoneIndex(3), BoneIndex(0), 1.0).with_rotation()],
1666            )
1667            .unwrap(),
1668        );
1669        let mut runtime = RuntimeInstance::new(model);
1670
1671        runtime.evaluate_current_pose();
1672
1673        assert_vec3a_near(
1674            translation(runtime.world_matrices()[4]),
1675            Vec3A::new(0.0, 1.0, 0.0),
1676        );
1677    }
1678
1679    #[test]
1680    fn scratch_ik_capacities_stable_after_repeated_evaluate() {
1681        let model = Arc::new(
1682            ModelArena::new_with_ik(
1683                vec![
1684                    BoneInit::new(None, Vec3A::ZERO),
1685                    BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1686                    BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1687                    BoneInit::new(None, Vec3A::ZERO),
1688                    BoneInit::new(Some(BoneIndex(3)), Vec3A::new(1.0, 0.0, 0.0)),
1689                    BoneInit::new(Some(BoneIndex(4)), Vec3A::new(1.0, 0.0, 0.0)),
1690                    BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1691                ],
1692                vec![
1693                    IkSolverInit {
1694                        ik_bone: BoneIndex(2),
1695                        target_bone: BoneIndex(1),
1696                        links: vec![IkLinkInit::new(BoneIndex(0))],
1697                        iteration_count: 1,
1698                        limit_angle: 0.0,
1699                    },
1700                    IkSolverInit {
1701                        ik_bone: BoneIndex(6),
1702                        target_bone: BoneIndex(5),
1703                        links: vec![IkLinkInit::new(BoneIndex(3)), IkLinkInit::new(BoneIndex(4))],
1704                        iteration_count: 1,
1705                        limit_angle: 0.0,
1706                    },
1707                ],
1708            )
1709            .unwrap(),
1710        );
1711        let mut runtime = RuntimeInstance::new(model);
1712
1713        runtime.evaluate_current_pose();
1714
1715        let cap_links = runtime.ik_scratch.links.capacity();
1716        let cap_base = runtime.ik_scratch.base_rotations.capacity();
1717        let cap_ik = runtime.ik_scratch.ik_rotations.capacity();
1718        let cap_best = runtime.ik_scratch.best_ik_rotations.capacity();
1719        let cap_chain = runtime.ik_scratch.chain_states.capacity();
1720
1721        for _ in 0..10 {
1722            runtime.evaluate_current_pose();
1723        }
1724
1725        assert_eq!(runtime.ik_scratch.links.capacity(), cap_links);
1726        assert_eq!(runtime.ik_scratch.base_rotations.capacity(), cap_base);
1727        assert_eq!(runtime.ik_scratch.ik_rotations.capacity(), cap_ik);
1728        assert_eq!(runtime.ik_scratch.best_ik_rotations.capacity(), cap_best);
1729        assert_eq!(runtime.ik_scratch.chain_states.capacity(), cap_chain);
1730    }
1731
1732    #[test]
1733    fn scratch_morph_capacity_stable_after_repeated_clip_frame() {
1734        let model = Arc::new(
1735            ModelArena::new_with_morphs(
1736                vec![
1737                    BoneInit::new(None, Vec3A::ZERO),
1738                    BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 1.0, 0.0)),
1739                ],
1740                Vec::new(),
1741                Vec::new(),
1742                crate::MorphInit {
1743                    morph_count: 2,
1744                    bone_offsets: vec![crate::BoneMorphOffset {
1745                        target_bone: BoneIndex(1),
1746                        position_offset: Vec3A::new(0.0, 0.0, 2.0),
1747                        rotation_offset: Quat::IDENTITY,
1748                    }],
1749                    bone_spans: vec![
1750                        crate::MorphOffsetSpan { start: 0, count: 1 },
1751                        crate::MorphOffsetSpan::default(),
1752                    ],
1753                    group_offsets: vec![crate::GroupMorphOffset {
1754                        child_morph: crate::MorphIndex(0),
1755                        ratio: 0.5,
1756                    }],
1757                    group_spans: vec![
1758                        crate::MorphOffsetSpan::default(),
1759                        crate::MorphOffsetSpan { start: 0, count: 1 },
1760                    ],
1761                    ..crate::MorphInit::default()
1762                },
1763            )
1764            .unwrap(),
1765        );
1766        let clip = AnimationClip::new_with_morphs(
1767            Vec::new(),
1768            vec![crate::MorphAnimationBinding {
1769                morph: crate::MorphIndex(1),
1770                track: crate::MorphTrack::from_keyframes(vec![
1771                    crate::MorphKeyframe::new(0, 0.0),
1772                    crate::MorphKeyframe::new(10, 1.0),
1773                ]),
1774            }],
1775        );
1776        let mut runtime = RuntimeInstance::new_with_morph_count(model, 2);
1777
1778        runtime.evaluate_clip_frame(&clip, 5.0);
1779
1780        let cap_expanded = runtime.morph_scratch.expanded_weights.capacity();
1781
1782        for _ in 0..10 {
1783            runtime.evaluate_clip_frame(&clip, 5.0);
1784        }
1785
1786        assert_eq!(
1787            runtime.morph_scratch.expanded_weights.capacity(),
1788            cap_expanded
1789        );
1790    }
1791}
mmd_anim_runtime/runtime.rs

mmd_anim_runtime/
runtime.rs
