mmd_anim_runtime/
runtime.rs

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

mmd_anim_runtime/
runtime.rs
