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 base_ik_rotations: Vec<Quat>,
26 ik_rotations: Vec<Quat>,
27 best_ik_rotations: Vec<Quat>,
28 chain_states: Vec<ChainLinkState>,
29}
30
31impl IkScratch {
32 fn new(model: &ModelArena) -> Self {
33 let max_links = model
34 .ik_solvers()
35 .iter()
36 .map(|s| s.links.len())
37 .max()
38 .unwrap_or(0);
39 IkScratch {
40 links: Vec::with_capacity(max_links),
41 base_rotations: Vec::with_capacity(max_links),
42 base_ik_rotations: Vec::with_capacity(max_links),
43 ik_rotations: Vec::with_capacity(max_links),
44 best_ik_rotations: Vec::with_capacity(max_links),
45 chain_states: Vec::with_capacity(max_links),
46 }
47 }
48}
49
50#[derive(Debug)]
51struct MorphScratch {
52 expanded_weights: Vec<f32>,
53}
54
55impl MorphScratch {
56 fn new(morph_count: usize) -> Self {
57 Self {
58 expanded_weights: vec![0.0; morph_count],
59 }
60 }
61}
62
63#[derive(Clone, Copy, Debug, Default, PartialEq)]
64pub struct IkSolverRuntimeStats {
65 pub solver_evaluations: u64,
66 pub configured_iterations: u64,
67 pub executed_iterations: u64,
68 pub tolerance_precheck_breaks: u64,
69 pub tolerance_post_iteration_breaks: u64,
70 pub rollback_breaks: u64,
71 pub max_iteration_exhaustions: u64,
72 pub link_visits: u64,
73 pub link_steps: u64,
74 pub final_distance_sum: f64,
75 pub final_distance_max: f32,
76 pub exhausted_final_distance_sum: f64,
77 pub exhausted_final_distance_max: f32,
78}
79
80impl IkSolverRuntimeStats {
81 fn reset(&mut self) {
82 *self = Self::default();
83 }
84}
85
86#[derive(Clone, Copy, Debug, PartialEq)]
87pub struct IkSolveOptions {
88 pub tolerance: f32,
89 pub max_iterations_cap: Option<u32>,
90}
91
92impl Default for IkSolveOptions {
93 fn default() -> Self {
94 Self {
95 tolerance: 0.0,
96 max_iterations_cap: None,
97 }
98 }
99}
100
101#[derive(Debug)]
102pub struct RuntimeInstance {
103 model: Arc<ModelArena>,
104 pose: PoseArena,
105 ik_scratch: IkScratch,
106 morph_scratch: MorphScratch,
107 ik_stats: Vec<IkSolverRuntimeStats>,
108 #[cfg(test)]
109 world_matrix_bone_update_count: usize,
110}
111
112impl RuntimeInstance {
113 pub fn new(model: Arc<ModelArena>) -> Self {
114 let morph_count = model.morph_count() as usize;
115 Self::new_with_morph_count(model, morph_count)
116 }
117
118 pub fn new_with_morph_count(model: Arc<ModelArena>, morph_count: usize) -> Self {
119 let ik_count = model.ik_count();
120 Self::new_with_counts(model, morph_count, ik_count)
121 }
122
123 pub fn new_with_counts(model: Arc<ModelArena>, morph_count: usize, ik_count: usize) -> Self {
124 let morph_count = morph_count.max(model.morph_count() as usize);
125 let pose = PoseArena::new_with_counts(model.bone_count(), morph_count, ik_count);
126 let ik_scratch = IkScratch::new(&model);
127 let morph_scratch = MorphScratch::new(morph_count);
128 let ik_stats = vec![IkSolverRuntimeStats::default(); model.ik_count()];
129 Self {
130 model,
131 pose,
132 ik_scratch,
133 morph_scratch,
134 ik_stats,
135 #[cfg(test)]
136 world_matrix_bone_update_count: 0,
137 }
138 }
139
140 #[inline]
141 pub fn model(&self) -> &ModelArena {
142 &self.model
143 }
144
145 #[inline]
146 pub fn pose(&self) -> &PoseArena {
147 &self.pose
148 }
149
150 #[inline]
151 pub fn pose_mut(&mut self) -> &mut PoseArena {
152 &mut self.pose
153 }
154
155 pub fn evaluate_current_pose(&mut self) {
156 self.pose.reset_ik_rotations();
157 self.evaluate_current_pose_ordered(IkSolveOptions::default());
158 }
159
160 pub fn evaluate_current_pose_with_ik_options(&mut self, options: IkSolveOptions) {
161 self.pose.reset_ik_rotations();
162 self.evaluate_current_pose_ordered(options);
163 }
164
165 pub fn evaluate_current_pose_without_ik(&mut self) {
169 self.pose.reset_ik_rotations();
170 self.update_world_matrices();
171 }
172
173 fn update_world_matrices(&mut self) {
174 self.update_world_matrices_from_eval_order_position(0);
175 }
176
177 fn update_world_matrices_from_eval_order_position(&mut self, start_position: usize) {
178 self.update_world_matrices_from_eval_order_position_for_phase(start_position, None);
179 }
180
181 fn update_world_matrices_from_eval_order_position_for_phase(
182 &mut self,
183 start_position: usize,
184 phase: Option<bool>,
185 ) {
186 let start_position =
187 self.expand_update_start_for_append_dependencies(start_position, phase);
188 for bone in &self.model.eval_order()[start_position..] {
189 if !self.bone_matches_phase(*bone, phase) {
190 continue;
191 }
192 self.pose.reset_append_transform(*bone);
193 }
194 for position in start_position..self.model.eval_order().len() {
195 let bone = self.model.eval_order()[position];
196 if !self.bone_matches_phase(bone, phase) {
197 continue;
198 }
199 self.update_append_transform_for_bone(bone);
200 self.update_world_matrix_for_bone(bone);
201 }
202 }
203
204 fn update_world_matrices_using_current_append_from_eval_order_position(
205 &mut self,
206 start_position: usize,
207 ) {
208 self.update_world_matrices_using_current_append_from_eval_order_position_for_phase(
209 start_position,
210 None,
211 );
212 }
213
214 fn update_world_matrices_using_current_append_from_eval_order_position_for_phase(
215 &mut self,
216 start_position: usize,
217 phase: Option<bool>,
218 ) {
219 for position in start_position..self.model.eval_order().len() {
220 let bone = self.model.eval_order()[position];
221 if !self.bone_matches_phase(bone, phase) {
222 continue;
223 }
224 self.update_world_matrix_for_bone(bone);
225 }
226 }
227
228 #[inline]
229 fn bone_matches_phase(&self, bone: crate::BoneIndex, phase: Option<bool>) -> bool {
230 phase.is_none_or(|after_physics| self.model.transform_after_physics(bone) == after_physics)
231 }
232
233 fn update_append_transform_for_bone(&mut self, bone: crate::BoneIndex) {
234 let Some(append_index) = self.model.append_transform_index(bone) else {
235 return;
236 };
237 let append = self.model.append_transform(append_index);
238 let use_source_append = !append.local
239 && self
240 .model
241 .append_transform_index(append.source_bone)
242 .is_some();
243 let mut source_rotation = if use_source_append {
244 self.pose.append_rotation(append.source_bone)
245 } else {
246 self.pose.local_rotation(append.source_bone)
247 };
248 if use_source_append && self.model.is_ik_link_bone(append.source_bone) {
249 source_rotation =
250 (self.pose.ik_rotation(append.source_bone) * source_rotation).normalize();
251 }
252 let source_position_offset = if use_source_append {
253 self.pose.append_position_offset(append.source_bone)
254 } else {
255 self.pose.local_position_offset(append.source_bone)
256 };
257 let append_output = solve_append_transform(AppendPrimitiveInput {
258 source_position_offset,
259 source_rotation,
260 ratio: append.ratio,
261 affect_rotation: append.affect_rotation,
262 affect_translation: append.affect_translation,
263 });
264 self.pose.set_append_rotation(bone, append_output.rotation);
265 self.pose
266 .set_append_position_offset(bone, append_output.position_offset);
267 }
268
269 fn update_world_matrix_for_bone(&mut self, bone: crate::BoneIndex) {
270 #[cfg(test)]
271 {
272 self.world_matrix_bone_update_count += 1;
273 }
274 let mut local_position =
275 self.model.rest_position(bone) + self.pose.local_position_offset(bone);
276 let mut local_rotation = self.pose.local_rotation(bone);
277 let local_scale = self.pose.local_scale(bone);
278
279 if let Some(append_index) = self.model.append_transform_index(bone) {
280 let append = self.model.append_transform(append_index);
281 if append.affect_rotation {
282 local_rotation = (local_rotation * self.pose.append_rotation(bone)).normalize();
283 }
284 if append.affect_translation {
285 local_position += self.pose.append_position_offset(bone);
286 }
287 }
288
289 if let Some(axis) = self.model.fixed_axis_constraint(bone) {
290 local_rotation = constrain_rotation_to_axis(local_rotation, axis);
291 }
292
293 let local_matrix = Mat4::from_scale_rotation_translation(
294 local_scale.into(),
295 local_rotation,
296 local_position.into(),
297 );
298
299 let world_matrix = match self.model.parent_index(bone) {
300 Some(parent) => self.pose.world_matrices()[parent.as_usize()] * local_matrix,
301 None => local_matrix,
302 };
303
304 self.pose.set_world_matrix(bone, world_matrix);
305 self.pose
306 .set_skinning_matrix(bone, world_matrix * self.model.inverse_bind_matrix(bone));
307 }
308
309 fn expand_update_start_for_append_dependencies(
310 &self,
311 start_position: usize,
312 phase: Option<bool>,
313 ) -> usize {
314 let mut start = start_position;
315 loop {
316 let mut changed = false;
317 for append in self.model.append_transforms() {
318 if !self.bone_matches_phase(append.target_bone, phase) {
319 continue;
320 }
321 let source_position = self.model.eval_order_position(append.source_bone);
322 let target_position = self.model.eval_order_position(append.target_bone);
323 if source_position >= start && target_position < start {
324 start = target_position;
325 changed = true;
326 }
327 }
328 if !changed {
329 return start;
330 }
331 }
332 }
333
334 fn evaluate_current_pose_ordered(&mut self, options: IkSolveOptions) {
335 self.pose.reset_append_transforms();
336 self.update_world_matrices_using_current_append_from_eval_order_position(0);
337
338 for after_physics in [false, true] {
339 for position in 0..self.model.eval_order().len() {
340 let bone = self.model.eval_order()[position];
341 if self.model.transform_after_physics(bone) != after_physics {
342 continue;
343 }
344
345 if self.model.append_transform_index(bone).is_some() {
346 self.pose.reset_append_transform(bone);
347 self.update_append_transform_for_bone(bone);
348 }
349 self.update_world_matrix_for_bone(bone);
350
351 for ik_index in 0..self.model.ik_count() {
352 if self.model.ik_solvers()[ik_index].ik_bone == bone {
353 self.solve_ik_solver(ik_index, options, after_physics);
354 }
355 }
356 }
357 }
358 self.update_world_matrices_using_current_append_from_eval_order_position(0);
359 }
360
361 fn solve_ik_solver(&mut self, ik_index: usize, options: IkSolveOptions, after_physics: bool) {
362 if self.pose.ik_enabled()[ik_index] == 0 {
363 return;
364 }
365
366 let tolerance = options.tolerance.max(0.0);
367 let mut links = std::mem::take(&mut self.ik_scratch.links);
368 let mut base_rotations = std::mem::take(&mut self.ik_scratch.base_rotations);
369 let mut base_ik_rotations = std::mem::take(&mut self.ik_scratch.base_ik_rotations);
370 let mut ik_rotations = std::mem::take(&mut self.ik_scratch.ik_rotations);
371 let mut best_ik_rotations = std::mem::take(&mut self.ik_scratch.best_ik_rotations);
372 let mut chain_states = std::mem::take(&mut self.ik_scratch.chain_states);
373
374 {
375 let solver = &self.model.ik_solvers()[ik_index];
376 let ik_bone = solver.ik_bone;
377 let target_bone = solver.target_bone;
378 let iteration_count = options
379 .max_iterations_cap
380 .map(|cap| solver.iteration_count.min(cap))
381 .unwrap_or(solver.iteration_count)
382 .max(1) as usize;
383 let limit_angle = solver.limit_angle.max(0.0);
384 let link_count = solver.links.len();
385
386 links.clear();
387 links.extend(solver.links.iter().cloned());
388 self.ik_stats[ik_index].solver_evaluations += 1;
389 self.ik_stats[ik_index].configured_iterations += iteration_count as u64;
390
391 base_rotations.clear();
392 base_rotations.extend(links.iter().map(|l| self.pose.local_rotation(l.bone)));
393 base_ik_rotations.clear();
394 base_ik_rotations.extend(links.iter().map(|l| self.pose.ik_rotation(l.bone)));
395 ik_rotations.clear();
396 ik_rotations.resize(link_count, Quat::IDENTITY);
397 best_ik_rotations.clear();
398 best_ik_rotations.resize(link_count, Quat::IDENTITY);
399 chain_states.clear();
400 chain_states.resize_with(link_count, || ChainLinkState {
401 previous_euler: [0.0; 3],
402 plane_mode_angle: 0.0,
403 });
404
405 self.apply_ik_link_rotations(
407 &links,
408 &base_rotations,
409 &base_ik_rotations,
410 &ik_rotations,
411 );
412 self.update_world_matrices_after_ik_link_change(
413 &links,
414 ik_bone,
415 target_bone,
416 Some(after_physics),
417 );
418
419 let mut broke_early = false;
420 let mut final_distance = f32::MAX;
421 let mut best_distance = f32::MAX;
422 for _iteration in 0..iteration_count {
423 let eff_pos = translation(self.pose.world_matrices()[target_bone.as_usize()]);
425 let ik_pos = translation(self.pose.world_matrices()[ik_bone.as_usize()]);
426 final_distance = (eff_pos - ik_pos).length();
427 if final_distance <= tolerance {
428 self.ik_stats[ik_index].tolerance_precheck_breaks += 1;
429 broke_early = true;
430 break;
431 }
432 self.ik_stats[ik_index].executed_iterations += 1;
433
434 for link_index in 0..link_count {
435 let link = &links[link_index];
436 let link_bone = link.bone;
437 self.ik_stats[ik_index].link_visits += 1;
438
439 if link_bone == target_bone {
440 continue;
441 }
442
443 let link_world = self.pose.world_matrices()[link_bone.as_usize()];
444 let link_pos = translation(link_world);
445 let eff_pos = translation(self.pose.world_matrices()[target_bone.as_usize()]);
446 let ik_pos = translation(self.pose.world_matrices()[ik_bone.as_usize()]);
447
448 let link_world_rot = rotation(link_world);
450 let local_effector = link_world_rot.inverse().mul_vec3a(eff_pos - link_pos);
451 let local_target = link_world_rot.inverse().mul_vec3a(ik_pos - link_pos);
452
453 if local_effector.length_squared() <= f32::EPSILON
454 || local_target.length_squared() <= f32::EPSILON
455 {
456 continue;
457 }
458
459 solve_link_step(LinkStepInput {
460 local_effector: &local_effector,
461 local_target: &local_target,
462 link_index,
463 base_rotations: &base_rotations,
464 ik_rotations: &mut ik_rotations,
465 chain_states: &mut chain_states,
466 angle_limit: link.angle_limit,
467 iteration: _iteration,
468 limit_angle,
469 });
470
471 self.apply_ik_link_rotations(
472 &links,
473 &base_rotations,
474 &base_ik_rotations,
475 &ik_rotations,
476 );
477 self.update_world_matrices_after_ik_link_change(
478 &links,
479 ik_bone,
480 target_bone,
481 Some(after_physics),
482 );
483 self.ik_stats[ik_index].link_steps += 1;
484 }
485
486 let current_distance = {
488 let eff = translation(self.pose.world_matrices()[target_bone.as_usize()]);
489 let ik = translation(self.pose.world_matrices()[ik_bone.as_usize()]);
490 (eff - ik).length()
491 };
492 final_distance = current_distance;
493
494 if current_distance < best_distance {
495 best_distance = current_distance;
496 best_ik_rotations.copy_from_slice(&ik_rotations);
497 if current_distance <= tolerance {
498 self.ik_stats[ik_index].tolerance_post_iteration_breaks += 1;
499 broke_early = true;
500 break;
501 }
502 } else {
503 self.ik_stats[ik_index].rollback_breaks += 1;
504 ik_rotations.copy_from_slice(&best_ik_rotations);
505 self.apply_ik_link_rotations(
506 &links,
507 &base_rotations,
508 &base_ik_rotations,
509 &ik_rotations,
510 );
511 self.update_world_matrices_after_ik_link_change(
512 &links,
513 ik_bone,
514 target_bone,
515 Some(after_physics),
516 );
517 broke_early = true;
518 break;
519 }
520 }
521 self.ik_stats[ik_index].final_distance_sum += f64::from(final_distance);
522 self.ik_stats[ik_index].final_distance_max = self.ik_stats[ik_index]
523 .final_distance_max
524 .max(final_distance);
525 if !broke_early {
526 self.ik_stats[ik_index].max_iteration_exhaustions += 1;
527 self.ik_stats[ik_index].exhausted_final_distance_sum += f64::from(final_distance);
528 self.ik_stats[ik_index].exhausted_final_distance_max = self.ik_stats[ik_index]
529 .exhausted_final_distance_max
530 .max(final_distance);
531 }
532
533 self.apply_ik_link_rotations(
535 &links,
536 &base_rotations,
537 &base_ik_rotations,
538 &best_ik_rotations,
539 );
540 self.update_world_matrices_after_ik_link_change(
541 &links,
542 ik_bone,
543 target_bone,
544 Some(after_physics),
545 );
546 }
547
548 self.ik_scratch.links = links;
549 self.ik_scratch.base_rotations = base_rotations;
550 self.ik_scratch.base_ik_rotations = base_ik_rotations;
551 self.ik_scratch.ik_rotations = ik_rotations;
552 self.ik_scratch.best_ik_rotations = best_ik_rotations;
553 self.ik_scratch.chain_states = chain_states;
554 }
555
556 fn update_world_matrices_after_ik_link_change(
557 &mut self,
558 links: &[crate::IkLink],
559 ik_bone: crate::BoneIndex,
560 target_bone: crate::BoneIndex,
561 phase: Option<bool>,
562 ) {
563 let start_position =
564 self.min_ik_dependency_eval_order_position(links, ik_bone, target_bone);
565 let start_position = self.expand_update_start_for_append_dependencies(start_position, None);
566 for position in start_position..self.model.eval_order().len() {
567 let bone = self.model.eval_order()[position];
568 if self.bone_matches_phase(bone, phase)
569 || self.bone_is_in_ik_update_scope(bone, links, ik_bone, target_bone)
570 || self.bone_depends_on_ik_update_scope_append_source(
571 bone,
572 links,
573 ik_bone,
574 target_bone,
575 )
576 {
577 self.update_append_transform_for_bone(bone);
578 self.update_world_matrix_for_bone(bone);
579 }
580 }
581 }
582
583 fn min_ik_dependency_eval_order_position(
584 &self,
585 links: &[crate::IkLink],
586 ik_bone: crate::BoneIndex,
587 target_bone: crate::BoneIndex,
588 ) -> usize {
589 let mut min_position = self.model.eval_order_position(ik_bone);
590 min_position = min_position.min(self.model.eval_order_position(target_bone));
591 for link in links {
592 min_position = min_position.min(self.model.eval_order_position(link.bone));
593 }
594 for bone in [ik_bone, target_bone]
595 .into_iter()
596 .chain(links.iter().map(|link| link.bone))
597 {
598 let mut current = Some(bone);
599 while let Some(parent) = current {
600 min_position = min_position.min(self.model.eval_order_position(parent));
601 current = self.model.parent_index(parent);
602 }
603 }
604 min_position
605 }
606
607 fn bone_is_in_ik_update_scope(
608 &self,
609 bone: crate::BoneIndex,
610 links: &[crate::IkLink],
611 ik_bone: crate::BoneIndex,
612 target_bone: crate::BoneIndex,
613 ) -> bool {
614 if bone == ik_bone || bone == target_bone || links.iter().any(|link| link.bone == bone) {
615 return true;
616 }
617 if self.bone_is_ancestor_of(bone, ik_bone) || self.bone_is_ancestor_of(bone, target_bone) {
618 return true;
619 }
620 links.iter().any(|link| {
621 self.bone_is_ancestor_of(bone, link.bone) || self.bone_is_ancestor_of(link.bone, bone)
622 })
623 }
624
625 fn bone_depends_on_ik_update_scope_append_source(
626 &self,
627 bone: crate::BoneIndex,
628 links: &[crate::IkLink],
629 ik_bone: crate::BoneIndex,
630 target_bone: crate::BoneIndex,
631 ) -> bool {
632 let mut changed_append_roots = Vec::new();
633 loop {
634 let mut changed = false;
635 for append in self.model.append_transforms() {
636 let source_changed = self.bone_is_in_ik_update_scope(
637 append.source_bone,
638 links,
639 ik_bone,
640 target_bone,
641 ) || changed_append_roots.iter().any(|root| {
642 append.source_bone == *root
643 || self.bone_is_ancestor_of(*root, append.source_bone)
644 });
645 if source_changed && !changed_append_roots.contains(&append.target_bone) {
646 changed_append_roots.push(append.target_bone);
647 changed = true;
648 }
649 }
650 if !changed {
651 break;
652 }
653 }
654
655 changed_append_roots
656 .iter()
657 .any(|root| bone == *root || self.bone_is_ancestor_of(*root, bone))
658 }
659
660 fn bone_is_ancestor_of(&self, ancestor: crate::BoneIndex, bone: crate::BoneIndex) -> bool {
661 let mut current = self.model.parent_index(bone);
662 while let Some(parent) = current {
663 if parent == ancestor {
664 return true;
665 }
666 current = self.model.parent_index(parent);
667 }
668 false
669 }
670
671 fn apply_ik_link_rotations(
672 &mut self,
673 links: &[crate::IkLink],
674 base_rotations: &[Quat],
675 base_ik_rotations: &[Quat],
676 ik_rotations: &[Quat],
677 ) {
678 for (i, link) in links.iter().enumerate() {
679 let effective = (ik_rotations[i] * base_rotations[i]).normalize();
680 let total_ik = (ik_rotations[i] * base_ik_rotations[i]).normalize();
681 self.pose.set_ik_rotation(link.bone, total_ik);
682 self.pose.set_local_rotation(link.bone, effective);
683 }
684 }
685
686 pub fn evaluate_rest_pose(&mut self) {
687 self.pose.reset_local_pose();
688 self.evaluate_current_pose();
689 }
690
691 pub fn evaluate_clip_frame(&mut self, clip: &AnimationClip, frame: f32) {
692 clip.apply_to_pose(frame, &mut self.pose);
693 self.expand_morphs();
694 self.evaluate_current_pose();
695 }
696
697 pub fn evaluate_clip_frame_with_ik_options(
698 &mut self,
699 clip: &AnimationClip,
700 frame: f32,
701 options: IkSolveOptions,
702 ) {
703 clip.apply_to_pose(frame, &mut self.pose);
704 self.expand_morphs();
705 self.evaluate_current_pose_with_ik_options(options);
706 }
707
708 pub fn evaluate_clip_frame_without_ik(&mut self, clip: &AnimationClip, frame: f32) {
713 clip.apply_to_pose(frame, &mut self.pose);
714 self.expand_morphs();
715 self.pose.reset_ik_rotations();
716 self.update_world_matrices();
717 }
718
719 pub fn expand_morphs(&mut self) {
725 self.expand_group_morphs();
726 self.apply_bone_morphs();
727 }
728
729 fn expand_group_morphs(&mut self) {
734 let spans = self.model.group_morph_spans();
735 let offsets = self.model.group_morph_offsets();
736 if spans.is_empty() || offsets.is_empty() {
737 return;
738 }
739 let mc = self.model.morph_count() as usize;
740 self.morph_scratch.expanded_weights.clear();
741 self.morph_scratch
742 .expanded_weights
743 .extend_from_slice(&self.pose.morph_weights()[..mc]);
744
745 for (morph_idx, &w) in self.pose.morph_weights()[..mc].iter().enumerate() {
746 if w == 0.0 {
747 continue;
748 }
749 expand_group_morph_weight(
750 morph_idx,
751 w,
752 spans,
753 offsets,
754 &mut self.morph_scratch.expanded_weights,
755 );
756 }
757 for (i, &w) in self.morph_scratch.expanded_weights.iter().enumerate() {
758 self.pose.set_morph_weight(MorphIndex(i as u32), w);
759 }
760 }
761
762 fn apply_bone_morphs(&mut self) {
765 let spans = self.model.bone_morph_spans();
766 let offsets = self.model.bone_morph_offsets();
767 if spans.is_empty() || offsets.is_empty() {
768 return;
769 }
770 for (morph_idx, span) in spans.iter().enumerate() {
771 let weight = self.pose.morph_weight(MorphIndex(morph_idx as u32));
772 if weight == 0.0 {
773 continue;
774 }
775 for i in span.start..span.start + span.count {
776 let off = &offsets[i as usize];
777 let pos = self.pose.local_position_offset(off.target_bone);
778 self.pose
779 .set_local_position_offset(off.target_bone, pos + off.position_offset * weight);
780 let rot = self.pose.local_rotation(off.target_bone);
781 let scaled = Quat::IDENTITY.slerp(off.rotation_offset, weight);
782 self.pose
783 .set_local_rotation(off.target_bone, (rot * scaled).normalize());
784 }
785 }
786 }
787
788 #[inline]
789 pub fn world_matrices(&self) -> &[Mat4] {
790 self.pose.world_matrices()
791 }
792
793 #[cfg(test)]
794 fn reset_world_matrix_bone_update_count(&mut self) {
795 self.world_matrix_bone_update_count = 0;
796 }
797
798 #[cfg(test)]
799 fn world_matrix_bone_update_count(&self) -> usize {
800 self.world_matrix_bone_update_count
801 }
802
803 #[inline]
804 pub fn skinning_matrices(&self) -> &[Mat4] {
805 self.pose.skinning_matrices()
806 }
807
808 #[inline]
809 pub fn morph_weights(&self) -> &[f32] {
810 self.pose.morph_weights()
811 }
812
813 pub fn reset_ik_runtime_stats(&mut self) {
814 for stats in &mut self.ik_stats {
815 stats.reset();
816 }
817 }
818
819 pub fn ik_runtime_stats(&self) -> &[IkSolverRuntimeStats] {
820 &self.ik_stats
821 }
822
823 #[inline]
824 pub fn ik_enabled(&self) -> &[u8] {
825 self.pose.ik_enabled()
826 }
827}
828
829fn expand_group_morph_weight(
830 morph_idx: usize,
831 weight: f32,
832 spans: &[crate::MorphOffsetSpan],
833 offsets: &[crate::GroupMorphOffset],
834 expanded_weights: &mut [f32],
835) {
836 let span = spans[morph_idx];
837 for i in span.start..span.start + span.count {
838 let off = &offsets[i as usize];
839 let child = off.child_morph.as_usize();
840 let contribution = weight * off.ratio;
841 expanded_weights[child] += contribution;
842 if spans[child].count > 0 {
843 expand_group_morph_weight(child, contribution, spans, offsets, expanded_weights);
844 }
845 }
846}
847
848#[cfg(test)]
849mod tests {
850 use std::sync::Arc;
851
852 use glam::{Quat, Vec3A};
853
854 use crate::{
855 AnimationClip, AppendTransformInit, BoneAnimationBinding, BoneIndex, BoneInit,
856 IkAngleLimit, IkLinkInit, IkSolverInit, ModelArena, MovableBoneKeyframe, MovableBoneTrack,
857 RuntimeInstance,
858 };
859
860 fn translation(matrix: glam::Mat4) -> Vec3A {
861 Vec3A::from_vec4(matrix.w_axis)
862 }
863
864 fn assert_vec3a_near(actual: Vec3A, expected: Vec3A) {
865 let delta = (actual - expected).abs();
866 assert!(
867 delta.x < 1.0e-5 && delta.y < 1.0e-5 && delta.z < 1.0e-5,
868 "actual={actual:?} expected={expected:?} delta={delta:?}"
869 );
870 }
871
872 #[test]
873 fn evaluates_rest_pose_world_matrices() {
874 let model = Arc::new(
875 ModelArena::new(vec![
876 BoneInit::new(None, Vec3A::new(1.0, 0.0, 0.0)),
877 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 2.0, 0.0)),
878 ])
879 .unwrap(),
880 );
881 let mut runtime = RuntimeInstance::new(model);
882
883 runtime.evaluate_rest_pose();
884
885 assert_vec3a_near(
886 translation(runtime.world_matrices()[0]),
887 Vec3A::new(1.0, 0.0, 0.0),
888 );
889 assert_vec3a_near(
890 translation(runtime.world_matrices()[1]),
891 Vec3A::new(1.0, 2.0, 0.0),
892 );
893 }
894
895 #[test]
896 fn evaluates_current_pose_with_parent_rotation() {
897 let model = Arc::new(
898 ModelArena::new(vec![
899 BoneInit::new(None, Vec3A::new(1.0, 0.0, 0.0)),
900 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 2.0, 0.0)),
901 ])
902 .unwrap(),
903 );
904 let mut runtime = RuntimeInstance::new(model);
905
906 runtime.pose_mut().set_local_rotation(
907 BoneIndex(0),
908 Quat::from_rotation_z(std::f32::consts::FRAC_PI_2),
909 );
910 runtime.evaluate_current_pose();
911
912 assert_vec3a_near(
913 translation(runtime.world_matrices()[1]),
914 Vec3A::new(-1.0, 0.0, 0.0),
915 );
916 }
917
918 #[test]
919 fn fixed_axis_bone_rotation_keeps_only_axis_twist() {
920 let model = Arc::new(
921 ModelArena::new(vec![
922 BoneInit::new(None, Vec3A::ZERO).with_fixed_axis(Vec3A::Y),
923 BoneInit::new(Some(BoneIndex(0)), Vec3A::X),
924 ])
925 .unwrap(),
926 );
927 let mut runtime = RuntimeInstance::new(model);
928
929 runtime.pose_mut().set_local_rotation(
930 BoneIndex(0),
931 (Quat::from_rotation_y(std::f32::consts::FRAC_PI_2)
932 * Quat::from_rotation_x(std::f32::consts::FRAC_PI_2))
933 .normalize(),
934 );
935 runtime.evaluate_current_pose();
936
937 assert_vec3a_near(
938 translation(runtime.world_matrices()[1]),
939 Vec3A::new(0.0, 0.0, -1.0),
940 );
941 }
942
943 #[test]
944 fn evaluates_current_pose_with_local_position_offset() {
945 let model = Arc::new(
946 ModelArena::new(vec![
947 BoneInit::new(None, Vec3A::new(1.0, 0.0, 0.0)),
948 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 2.0, 0.0)),
949 ])
950 .unwrap(),
951 );
952 let mut runtime = RuntimeInstance::new(model);
953
954 runtime
955 .pose_mut()
956 .set_local_position_offset(BoneIndex(1), Vec3A::new(0.0, 0.0, 3.0));
957 runtime.evaluate_current_pose();
958
959 assert_vec3a_near(
960 translation(runtime.world_matrices()[1]),
961 Vec3A::new(1.0, 2.0, 3.0),
962 );
963 }
964
965 #[test]
966 fn evaluates_clip_frame_into_world_matrices() {
967 let model = Arc::new(
968 ModelArena::new(vec![
969 BoneInit::new(None, Vec3A::new(1.0, 0.0, 0.0)),
970 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 2.0, 0.0)),
971 ])
972 .unwrap(),
973 );
974 let clip = AnimationClip::new(vec![BoneAnimationBinding {
975 bone: BoneIndex(1),
976 track: MovableBoneTrack::from_keyframes(vec![
977 MovableBoneKeyframe::new(0, Vec3A::ZERO, Quat::IDENTITY),
978 MovableBoneKeyframe::new(10, Vec3A::new(0.0, 0.0, 4.0), Quat::IDENTITY),
979 ]),
980 }]);
981 let mut runtime = RuntimeInstance::new(model);
982
983 runtime.evaluate_clip_frame(&clip, 5.0);
984
985 assert_vec3a_near(
986 translation(runtime.world_matrices()[1]),
987 Vec3A::new(1.0, 2.0, 2.0),
988 );
989 }
990
991 #[test]
992 fn applies_append_rotation_before_world_matrix_output() {
993 let model = Arc::new(
994 ModelArena::new_full(
995 vec![
996 BoneInit::new(None, Vec3A::ZERO),
997 BoneInit::new(None, Vec3A::ZERO),
998 BoneInit::new(Some(BoneIndex(1)), Vec3A::new(1.0, 0.0, 0.0)),
999 ],
1000 Vec::new(),
1001 vec![AppendTransformInit::new(BoneIndex(1), BoneIndex(0), 1.0).with_rotation()],
1002 )
1003 .unwrap(),
1004 );
1005 let mut runtime = RuntimeInstance::new(model);
1006
1007 runtime.pose_mut().set_local_rotation(
1008 BoneIndex(0),
1009 Quat::from_rotation_z(std::f32::consts::FRAC_PI_2),
1010 );
1011 runtime.evaluate_current_pose();
1012
1013 assert_vec3a_near(
1014 translation(runtime.world_matrices()[2]),
1015 Vec3A::new(0.0, 1.0, 0.0),
1016 );
1017 }
1018
1019 #[test]
1020 fn applies_append_translation_before_world_matrix_output() {
1021 let model = Arc::new(
1022 ModelArena::new_full(
1023 vec![
1024 BoneInit::new(None, Vec3A::ZERO),
1025 BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1026 ],
1027 Vec::new(),
1028 vec![AppendTransformInit::new(BoneIndex(1), BoneIndex(0), 0.5).with_translation()],
1029 )
1030 .unwrap(),
1031 );
1032 let mut runtime = RuntimeInstance::new(model);
1033
1034 runtime
1035 .pose_mut()
1036 .set_local_position_offset(BoneIndex(0), Vec3A::new(2.0, 0.0, 0.0));
1037 runtime.evaluate_current_pose();
1038
1039 assert_vec3a_near(
1040 translation(runtime.world_matrices()[1]),
1041 Vec3A::new(1.0, 1.0, 0.0),
1042 );
1043 }
1044
1045 #[test]
1046 fn initializes_ik_enabled_from_model_solvers() {
1047 let model = Arc::new(
1048 ModelArena::new_with_ik(
1049 vec![
1050 BoneInit::new(None, Vec3A::ZERO),
1051 BoneInit::new(Some(BoneIndex(0)), Vec3A::ZERO),
1052 ],
1053 vec![IkSolverInit::new(
1054 BoneIndex(1),
1055 BoneIndex(0),
1056 vec![IkLinkInit::new(BoneIndex(0))],
1057 )],
1058 )
1059 .unwrap(),
1060 );
1061
1062 let runtime = RuntimeInstance::new(model);
1063
1064 assert_eq!(runtime.ik_enabled(), &[1]);
1065 }
1066
1067 #[test]
1068 fn solves_one_link_ik_toward_controller_bone() {
1069 let model = Arc::new(
1070 ModelArena::new_with_ik(
1071 vec![
1072 BoneInit::new(None, Vec3A::ZERO),
1073 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1074 BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1075 ],
1076 vec![IkSolverInit {
1077 ik_bone: BoneIndex(2),
1078 target_bone: BoneIndex(1),
1079 links: vec![IkLinkInit::new(BoneIndex(0))],
1080 iteration_count: 1,
1081 limit_angle: 0.0,
1082 }],
1083 )
1084 .unwrap(),
1085 );
1086 let mut runtime = RuntimeInstance::new(model);
1087
1088 runtime.evaluate_current_pose();
1089
1090 assert_vec3a_near(
1091 translation(runtime.world_matrices()[1]),
1092 Vec3A::new(0.0, 1.0, 0.0),
1093 );
1094 }
1095
1096 #[test]
1097 fn skips_disabled_ik_solver() {
1098 let model = Arc::new(
1099 ModelArena::new_with_ik(
1100 vec![
1101 BoneInit::new(None, Vec3A::ZERO),
1102 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1103 BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1104 ],
1105 vec![IkSolverInit {
1106 ik_bone: BoneIndex(2),
1107 target_bone: BoneIndex(1),
1108 links: vec![IkLinkInit::new(BoneIndex(0))],
1109 iteration_count: 1,
1110 limit_angle: 0.0,
1111 }],
1112 )
1113 .unwrap(),
1114 );
1115 let mut runtime = RuntimeInstance::new(model);
1116
1117 runtime.pose_mut().set_ik_enabled(0, false);
1118 runtime.evaluate_current_pose();
1119
1120 assert_vec3a_near(
1121 translation(runtime.world_matrices()[1]),
1122 Vec3A::new(1.0, 0.0, 0.0),
1123 );
1124 }
1125
1126 #[test]
1127 fn solves_two_link_ik_chain_toward_controller_bone() {
1128 let model = Arc::new(
1129 ModelArena::new_with_ik(
1130 vec![
1131 BoneInit::new(None, Vec3A::ZERO),
1132 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1133 BoneInit::new(Some(BoneIndex(1)), Vec3A::new(1.0, 0.0, 0.0)),
1134 BoneInit::new(None, Vec3A::new(1.0, 1.0, 0.0)),
1135 ],
1136 vec![IkSolverInit {
1137 ik_bone: BoneIndex(3),
1138 target_bone: BoneIndex(2),
1139 links: vec![IkLinkInit::new(BoneIndex(1)), IkLinkInit::new(BoneIndex(0))],
1140 iteration_count: 4,
1141 limit_angle: 0.0,
1142 }],
1143 )
1144 .unwrap(),
1145 );
1146 let mut runtime = RuntimeInstance::new(model);
1147
1148 runtime.evaluate_current_pose();
1149
1150 assert_vec3a_near(
1151 translation(runtime.world_matrices()[2]),
1152 Vec3A::new(1.0, 1.0, 0.0),
1153 );
1154 }
1155
1156 #[test]
1157 fn evaluates_all_solvers_attached_to_same_ik_bone() {
1158 let model = Arc::new(
1159 ModelArena::new_with_ik(
1160 vec![
1161 BoneInit::new(None, Vec3A::ZERO),
1162 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1163 BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1164 ],
1165 vec![
1166 IkSolverInit {
1167 ik_bone: BoneIndex(2),
1168 target_bone: BoneIndex(1),
1169 links: vec![IkLinkInit::new(BoneIndex(0))],
1170 iteration_count: 1,
1171 limit_angle: 0.0,
1172 },
1173 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 )
1182 .unwrap(),
1183 );
1184 let mut runtime = RuntimeInstance::new(model);
1185
1186 runtime.evaluate_current_pose();
1187
1188 assert_eq!(runtime.ik_runtime_stats()[0].solver_evaluations, 1);
1189 assert_eq!(runtime.ik_runtime_stats()[1].solver_evaluations, 1);
1190 }
1191
1192 #[test]
1193 fn ik_updates_only_affected_eval_suffix_for_late_chain() {
1194 let unrelated_count = 96usize;
1195 let chain_root = BoneIndex(unrelated_count as u32);
1196 let chain_mid = BoneIndex(unrelated_count as u32 + 1);
1197 let chain_tip = BoneIndex(unrelated_count as u32 + 2);
1198 let controller = BoneIndex(unrelated_count as u32 + 3);
1199
1200 let mut bones = Vec::new();
1201 for i in 0..unrelated_count {
1202 bones.push(BoneInit::new(None, Vec3A::new(i as f32 * 10.0, -10.0, 0.0)));
1203 }
1204 bones.push(BoneInit::new(None, Vec3A::ZERO));
1205 bones.push(BoneInit::new(Some(chain_root), Vec3A::new(1.0, 0.0, 0.0)));
1206 bones.push(BoneInit::new(Some(chain_mid), Vec3A::new(1.0, 0.0, 0.0)));
1207 bones.push(BoneInit::new(None, Vec3A::new(1.0, 1.0, 0.0)));
1208
1209 let model = Arc::new(
1210 ModelArena::new_with_ik(
1211 bones,
1212 vec![IkSolverInit {
1213 ik_bone: controller,
1214 target_bone: chain_tip,
1215 links: vec![IkLinkInit::new(chain_mid), IkLinkInit::new(chain_root)],
1216 iteration_count: 4,
1217 limit_angle: 0.0,
1218 }],
1219 )
1220 .unwrap(),
1221 );
1222 let mut runtime = RuntimeInstance::new(model);
1223
1224 runtime.reset_world_matrix_bone_update_count();
1225 runtime.evaluate_current_pose();
1226
1227 assert_vec3a_near(
1228 translation(runtime.world_matrices()[chain_tip.as_usize()]),
1229 Vec3A::new(1.0, 1.0, 0.0),
1230 );
1231 assert!(
1232 runtime.world_matrix_bone_update_count() < 360,
1233 "IK should not recompute unrelated prefix bones repeatedly; updated {} bones",
1234 runtime.world_matrix_bone_update_count()
1235 );
1236 }
1237
1238 #[test]
1239 fn clamps_ik_rotation_by_solver_limit_angle() {
1240 let model = Arc::new(
1241 ModelArena::new_with_ik(
1242 vec![
1243 BoneInit::new(None, Vec3A::ZERO),
1244 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1245 BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1246 ],
1247 vec![IkSolverInit {
1248 ik_bone: BoneIndex(2),
1249 target_bone: BoneIndex(1),
1250 links: vec![IkLinkInit::new(BoneIndex(0))],
1251 iteration_count: 1,
1252 limit_angle: std::f32::consts::FRAC_PI_4,
1253 }],
1254 )
1255 .unwrap(),
1256 );
1257 let mut runtime = RuntimeInstance::new(model);
1258
1259 runtime.evaluate_current_pose();
1260
1261 let expected = Vec3A::new(
1262 std::f32::consts::FRAC_1_SQRT_2,
1263 std::f32::consts::FRAC_1_SQRT_2,
1264 0.0,
1265 );
1266 assert_vec3a_near(translation(runtime.world_matrices()[1]), expected);
1267 }
1268
1269 #[test]
1270 fn applies_constant_limit_angle_per_iteration() {
1271 let model = Arc::new(
1272 ModelArena::new_with_ik(
1273 vec![
1274 BoneInit::new(None, Vec3A::ZERO),
1275 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1276 BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1277 ],
1278 vec![IkSolverInit {
1279 ik_bone: BoneIndex(2),
1280 target_bone: BoneIndex(1),
1281 links: vec![IkLinkInit::new(BoneIndex(1)), IkLinkInit::new(BoneIndex(0))],
1282 iteration_count: 1,
1283 limit_angle: std::f32::consts::FRAC_PI_4,
1284 }],
1285 )
1286 .unwrap(),
1287 );
1288 let mut runtime = RuntimeInstance::new(model);
1289
1290 runtime.evaluate_current_pose();
1291
1292 let expected = Vec3A::new(
1296 std::f32::consts::FRAC_1_SQRT_2,
1297 std::f32::consts::FRAC_1_SQRT_2,
1298 0.0,
1299 );
1300 assert_vec3a_near(translation(runtime.world_matrices()[1]), expected);
1301 }
1302
1303 #[test]
1304 fn clip_frame_produces_deterministic_world_translations() {
1305 let model = Arc::new(
1306 ModelArena::new(vec![
1307 BoneInit::new(None, Vec3A::new(1.0, 0.0, 0.0)),
1308 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 2.0, 0.0)),
1309 ])
1310 .unwrap(),
1311 );
1312 let clip = AnimationClip::new(vec![BoneAnimationBinding {
1313 bone: BoneIndex(1),
1314 track: MovableBoneTrack::from_keyframes(vec![
1315 MovableBoneKeyframe::new(0, Vec3A::ZERO, Quat::IDENTITY),
1316 MovableBoneKeyframe::new(10, Vec3A::new(0.0, 0.0, 4.0), Quat::IDENTITY),
1317 ]),
1318 }]);
1319 let mut runtime = RuntimeInstance::new(model);
1320
1321 runtime.evaluate_clip_frame(&clip, 5.0);
1322
1323 let matrices = runtime.world_matrices();
1324 assert_eq!(matrices.len(), 2);
1325 assert_vec3a_near(translation(matrices[0]), Vec3A::new(1.0, 0.0, 0.0));
1326 assert_vec3a_near(translation(matrices[1]), Vec3A::new(1.0, 2.0, 2.0));
1327 }
1328
1329 #[test]
1330 fn evaluate_clip_frame_without_ik_leaves_ik_unsolved() {
1331 let model = Arc::new(
1332 ModelArena::new_with_ik(
1333 vec![
1334 BoneInit::new(None, Vec3A::ZERO),
1335 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1336 BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1337 ],
1338 vec![IkSolverInit {
1339 ik_bone: BoneIndex(2),
1340 target_bone: BoneIndex(1),
1341 links: vec![IkLinkInit::new(BoneIndex(0))],
1342 iteration_count: 1,
1343 limit_angle: 0.0,
1344 }],
1345 )
1346 .unwrap(),
1347 );
1348 let clip = AnimationClip::new(vec![]);
1349
1350 let mut without_ik = RuntimeInstance::new(Arc::clone(&model));
1351 let mut with_ik = RuntimeInstance::new(model);
1352
1353 without_ik.evaluate_clip_frame_without_ik(&clip, 0.0);
1354 with_ik.evaluate_clip_frame(&clip, 0.0);
1355
1356 assert_vec3a_near(
1358 translation(without_ik.world_matrices()[1]),
1359 Vec3A::new(1.0, 0.0, 0.0),
1360 );
1361 assert_vec3a_near(
1363 translation(with_ik.world_matrices()[1]),
1364 Vec3A::new(0.0, 1.0, 0.0),
1365 );
1366 }
1367
1368 #[test]
1369 fn ik_options_cap_configured_iterations() {
1370 let model = Arc::new(
1371 ModelArena::new_with_ik(
1372 vec![
1373 BoneInit::new(None, Vec3A::ZERO),
1374 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1375 BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1376 ],
1377 vec![IkSolverInit {
1378 ik_bone: BoneIndex(2),
1379 target_bone: BoneIndex(1),
1380 links: vec![IkLinkInit::new(BoneIndex(0))],
1381 iteration_count: 100,
1382 limit_angle: 0.0,
1383 }],
1384 )
1385 .unwrap(),
1386 );
1387 let mut runtime = RuntimeInstance::new(model);
1388
1389 runtime.reset_ik_runtime_stats();
1390 runtime.evaluate_current_pose_with_ik_options(super::IkSolveOptions {
1391 tolerance: 0.0,
1392 max_iterations_cap: Some(5),
1393 });
1394
1395 assert_eq!(runtime.ik_runtime_stats()[0].configured_iterations, 5);
1396 }
1397
1398 fn assert_near(actual: f32, expected: f32) {
1401 let delta = (actual - expected).abs();
1402 assert!(
1403 delta < 1.0e-5,
1404 "actual={actual:?} expected={expected:?} delta={delta:?}"
1405 );
1406 }
1407
1408 #[test]
1409 fn bone_morph_position_offset_drives_world_position() {
1410 let model = Arc::new(
1411 ModelArena::new_with_morphs(
1412 vec![
1413 BoneInit::new(None, Vec3A::ZERO),
1414 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 1.0, 0.0)),
1415 ],
1416 Vec::new(),
1417 Vec::new(),
1418 crate::MorphInit {
1419 morph_count: 1,
1420 bone_offsets: vec![crate::BoneMorphOffset {
1421 target_bone: BoneIndex(1),
1422 position_offset: Vec3A::new(0.0, 0.0, 2.0),
1423 rotation_offset: Quat::IDENTITY,
1424 }],
1425 bone_spans: vec![crate::MorphOffsetSpan { start: 0, count: 1 }],
1426 group_offsets: vec![],
1427 group_spans: vec![crate::MorphOffsetSpan::default()],
1428 ..crate::MorphInit::default()
1429 },
1430 )
1431 .unwrap(),
1432 );
1433 let clip = AnimationClip::new_with_morphs(
1434 Vec::new(),
1435 vec![crate::MorphAnimationBinding {
1436 morph: crate::MorphIndex(0),
1437 track: crate::MorphTrack::from_keyframes(vec![
1438 crate::MorphKeyframe::new(0, 0.0),
1439 crate::MorphKeyframe::new(10, 1.0),
1440 ]),
1441 }],
1442 );
1443 let mut runtime = RuntimeInstance::new_with_morph_count(model, 1);
1444
1445 runtime.evaluate_clip_frame(&clip, 5.0);
1446
1447 assert_vec3a_near(
1449 translation(runtime.world_matrices()[1]),
1450 Vec3A::new(0.0, 1.0, 1.0),
1451 );
1452 assert_near(runtime.morph_weights()[0], 0.5);
1453 }
1454
1455 #[test]
1456 fn bone_morph_rotation_offset_affects_child_position() {
1457 let model = Arc::new(
1458 ModelArena::new_with_morphs(
1459 vec![
1460 BoneInit::new(None, Vec3A::ZERO),
1461 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1462 BoneInit::new(Some(BoneIndex(1)), Vec3A::new(1.0, 0.0, 0.0)),
1463 ],
1464 Vec::new(),
1465 Vec::new(),
1466 crate::MorphInit {
1467 morph_count: 1,
1468 bone_offsets: vec![crate::BoneMorphOffset {
1469 target_bone: BoneIndex(1),
1470 position_offset: Vec3A::ZERO,
1471 rotation_offset: Quat::from_rotation_z(std::f32::consts::FRAC_PI_2),
1472 }],
1473 bone_spans: vec![crate::MorphOffsetSpan { start: 0, count: 1 }],
1474 group_offsets: vec![],
1475 group_spans: vec![crate::MorphOffsetSpan::default()],
1476 ..crate::MorphInit::default()
1477 },
1478 )
1479 .unwrap(),
1480 );
1481 let clip = AnimationClip::new_with_morphs(
1482 Vec::new(),
1483 vec![crate::MorphAnimationBinding {
1484 morph: crate::MorphIndex(0),
1485 track: crate::MorphTrack::from_keyframes(vec![
1486 crate::MorphKeyframe::new(0, 0.0),
1487 crate::MorphKeyframe::new(10, 1.0),
1488 ]),
1489 }],
1490 );
1491 let mut runtime = RuntimeInstance::new_with_morph_count(model, 1);
1492
1493 runtime.evaluate_clip_frame(&clip, 10.0);
1494
1495 assert_vec3a_near(
1498 translation(runtime.world_matrices()[2]),
1499 Vec3A::new(1.0, 1.0, 0.0),
1500 );
1501 }
1502
1503 #[test]
1504 fn group_morph_contributes_to_bone_morph_weight() {
1505 let model = Arc::new(
1508 ModelArena::new_with_morphs(
1509 vec![
1510 BoneInit::new(None, Vec3A::ZERO),
1511 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 1.0, 0.0)),
1512 ],
1513 Vec::new(),
1514 Vec::new(),
1515 crate::MorphInit {
1516 morph_count: 2,
1517 bone_offsets: vec![crate::BoneMorphOffset {
1518 target_bone: BoneIndex(1),
1519 position_offset: Vec3A::new(0.0, 0.0, 2.0),
1520 rotation_offset: Quat::IDENTITY,
1521 }],
1522 bone_spans: vec![
1523 crate::MorphOffsetSpan { start: 0, count: 1 },
1524 crate::MorphOffsetSpan::default(),
1525 ],
1526 group_offsets: vec![crate::GroupMorphOffset {
1527 child_morph: crate::MorphIndex(0),
1528 ratio: 0.5,
1529 }],
1530 group_spans: vec![
1531 crate::MorphOffsetSpan::default(),
1532 crate::MorphOffsetSpan { start: 0, count: 1 },
1533 ],
1534 ..crate::MorphInit::default()
1535 },
1536 )
1537 .unwrap(),
1538 );
1539 let clip = AnimationClip::new_with_morphs(
1541 Vec::new(),
1542 vec![crate::MorphAnimationBinding {
1543 morph: crate::MorphIndex(1),
1544 track: crate::MorphTrack::from_keyframes(vec![
1545 crate::MorphKeyframe::new(0, 0.0),
1546 crate::MorphKeyframe::new(10, 1.0),
1547 ]),
1548 }],
1549 );
1550 let mut runtime = RuntimeInstance::new_with_morph_count(model, 2);
1551
1552 runtime.evaluate_clip_frame(&clip, 10.0);
1553
1554 assert_near(runtime.morph_weights()[0], 0.5);
1557 assert_near(runtime.morph_weights()[1], 1.0);
1558 assert_vec3a_near(
1559 translation(runtime.world_matrices()[1]),
1560 Vec3A::new(0.0, 1.0, 1.0),
1561 );
1562 }
1563
1564 #[test]
1565 fn group_morph_can_reference_later_child_morph() {
1566 let model = Arc::new(
1567 ModelArena::new_with_morphs(
1568 vec![
1569 BoneInit::new(None, Vec3A::ZERO),
1570 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 1.0, 0.0)),
1571 ],
1572 Vec::new(),
1573 Vec::new(),
1574 crate::MorphInit {
1575 morph_count: 2,
1576 bone_offsets: vec![crate::BoneMorphOffset {
1577 target_bone: BoneIndex(1),
1578 position_offset: Vec3A::new(0.0, 0.0, 2.0),
1579 rotation_offset: Quat::IDENTITY,
1580 }],
1581 bone_spans: vec![
1582 crate::MorphOffsetSpan::default(),
1583 crate::MorphOffsetSpan { start: 0, count: 1 },
1584 ],
1585 group_offsets: vec![crate::GroupMorphOffset {
1586 child_morph: crate::MorphIndex(1),
1587 ratio: 0.5,
1588 }],
1589 group_spans: vec![
1590 crate::MorphOffsetSpan { start: 0, count: 1 },
1591 crate::MorphOffsetSpan::default(),
1592 ],
1593 ..crate::MorphInit::default()
1594 },
1595 )
1596 .unwrap(),
1597 );
1598 let clip = AnimationClip::new_with_morphs(
1599 Vec::new(),
1600 vec![crate::MorphAnimationBinding {
1601 morph: crate::MorphIndex(0),
1602 track: crate::MorphTrack::from_keyframes(vec![crate::MorphKeyframe::new(0, 1.0)]),
1603 }],
1604 );
1605 let mut runtime = RuntimeInstance::new(model);
1606
1607 runtime.evaluate_clip_frame(&clip, 0.0);
1608
1609 assert_near(runtime.morph_weights()[0], 1.0);
1610 assert_near(runtime.morph_weights()[1], 0.5);
1611 assert_vec3a_near(
1612 translation(runtime.world_matrices()[1]),
1613 Vec3A::new(0.0, 1.0, 1.0),
1614 );
1615 }
1616
1617 #[test]
1618 fn chained_group_morphs_descend_to_bone_morph_weight() {
1619 let model = Arc::new(
1620 ModelArena::new_with_morphs(
1621 vec![
1622 BoneInit::new(None, Vec3A::ZERO),
1623 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 1.0, 0.0)),
1624 ],
1625 Vec::new(),
1626 Vec::new(),
1627 crate::MorphInit {
1628 morph_count: 3,
1629 bone_offsets: vec![crate::BoneMorphOffset {
1630 target_bone: BoneIndex(1),
1631 position_offset: Vec3A::new(0.0, 0.0, 2.0),
1632 rotation_offset: Quat::IDENTITY,
1633 }],
1634 bone_spans: vec![
1635 crate::MorphOffsetSpan { start: 0, count: 1 },
1636 crate::MorphOffsetSpan::default(),
1637 crate::MorphOffsetSpan::default(),
1638 ],
1639 group_offsets: vec![
1640 crate::GroupMorphOffset {
1641 child_morph: crate::MorphIndex(0),
1642 ratio: 0.25,
1643 },
1644 crate::GroupMorphOffset {
1645 child_morph: crate::MorphIndex(1),
1646 ratio: 0.5,
1647 },
1648 ],
1649 group_spans: vec![
1650 crate::MorphOffsetSpan::default(),
1651 crate::MorphOffsetSpan { start: 0, count: 1 },
1652 crate::MorphOffsetSpan { start: 1, count: 1 },
1653 ],
1654 ..crate::MorphInit::default()
1655 },
1656 )
1657 .unwrap(),
1658 );
1659 let clip = AnimationClip::new_with_morphs(
1660 Vec::new(),
1661 vec![crate::MorphAnimationBinding {
1662 morph: crate::MorphIndex(2),
1663 track: crate::MorphTrack::from_keyframes(vec![crate::MorphKeyframe::new(0, 1.0)]),
1664 }],
1665 );
1666 let mut runtime = RuntimeInstance::new(model);
1667
1668 runtime.evaluate_clip_frame(&clip, 0.0);
1669
1670 assert_near(runtime.morph_weights()[2], 1.0);
1671 assert_near(runtime.morph_weights()[1], 0.5);
1672 assert_near(runtime.morph_weights()[0], 0.125);
1673 assert_vec3a_near(
1674 translation(runtime.world_matrices()[1]),
1675 Vec3A::new(0.0, 1.0, 0.25),
1676 );
1677 }
1678
1679 #[test]
1680 fn expand_morphs_noop_when_no_morph_defs() {
1681 let model = Arc::new(ModelArena::new(vec![BoneInit::new(None, Vec3A::ZERO)]).unwrap());
1682 let mut runtime = RuntimeInstance::new_with_morph_count(model, 1);
1683 runtime
1684 .pose_mut()
1685 .set_morph_weight(crate::MorphIndex(0), 1.0);
1686 runtime.expand_morphs();
1687 assert_near(runtime.morph_weights()[0], 1.0);
1689 }
1690
1691 #[test]
1692 fn clamps_link_local_rotation_to_angle_limit() {
1693 let model = Arc::new(
1694 ModelArena::new_with_ik(
1695 vec![
1696 BoneInit::new(None, Vec3A::ZERO),
1697 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1698 BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1699 ],
1700 vec![IkSolverInit {
1701 ik_bone: BoneIndex(2),
1702 target_bone: BoneIndex(1),
1703 links: vec![
1704 IkLinkInit::new(BoneIndex(0)).with_angle_limit(IkAngleLimit::new(
1705 Vec3A::new(0.0, 0.0, 0.0),
1706 Vec3A::new(0.0, 0.0, std::f32::consts::FRAC_PI_4),
1707 )),
1708 ],
1709 iteration_count: 1,
1710 limit_angle: 0.0,
1711 }],
1712 )
1713 .unwrap(),
1714 );
1715 let mut runtime = RuntimeInstance::new(model);
1716
1717 runtime.evaluate_current_pose();
1718
1719 let expected = Vec3A::new(
1720 std::f32::consts::FRAC_1_SQRT_2,
1721 std::f32::consts::FRAC_1_SQRT_2,
1722 0.0,
1723 );
1724 assert_vec3a_near(translation(runtime.world_matrices()[1]), expected);
1725 }
1726
1727 #[test]
1728 fn multi_axis_limited_link_solves_before_clamping() {
1729 let local_effector = Vec3A::X;
1730 let local_target = Vec3A::new(0.25, 0.55, 0.80).normalize();
1731 let limits = IkAngleLimit::new(Vec3A::new(0.0, -1.0, -1.0), Vec3A::new(0.0, 1.0, 1.0));
1732 let base_rotations = vec![Quat::IDENTITY];
1733 let mut ik_rotations = vec![Quat::IDENTITY];
1734 let mut chain_states = vec![super::ChainLinkState {
1735 previous_euler: [0.0; 3],
1736 plane_mode_angle: 0.0,
1737 }];
1738
1739 super::solve_limited_axes_link_step(super::LimitedAxesLinkStepInput {
1740 local_effector: &local_effector,
1741 local_target: &local_target,
1742 link_index: 0,
1743 base_rotations: &base_rotations,
1744 ik_rotations: &mut ik_rotations,
1745 chain_states: &mut chain_states,
1746 limits,
1747 limit_angle: 0.0,
1748 });
1749
1750 let current_direction = ik_rotations[0].mul_vec3a(local_effector).normalize();
1751 let legacy_direction =
1752 legacy_clamp_only_limited_direction(local_effector, local_target, limits);
1753 let current_error = (current_direction - local_target).length();
1754 let legacy_error = (legacy_direction - local_target).length();
1755
1756 assert!(
1757 current_error < legacy_error - 0.015,
1758 "current_error={current_error:.6} legacy_error={legacy_error:.6} current={current_direction:?} legacy={legacy_direction:?} target={local_target:?}"
1759 );
1760 assert!(
1761 chain_states[0].previous_euler[1].abs() > 0.1
1762 && chain_states[0].previous_euler[2].abs() > 0.1,
1763 "multi-axis limited IK should use both Y and Z axes; euler={:?}",
1764 chain_states[0].previous_euler
1765 );
1766 }
1767
1768 #[test]
1769 fn multi_axis_limited_link_applies_limits_to_total_rotation() {
1770 let local_effector = Vec3A::new(0.25, 0.45, 0.85).normalize();
1771 let local_target = Vec3A::new(0.55, 0.15, 0.80).normalize();
1772 let limits = IkAngleLimit::new(Vec3A::new(-1.0, -1.0, 0.0), Vec3A::new(1.0, 1.0, 0.0));
1773 let base_rotations = vec![Quat::from_rotation_z(0.45)];
1774 let mut ik_rotations = vec![Quat::IDENTITY];
1775 let mut chain_states = vec![super::ChainLinkState {
1776 previous_euler: [0.0; 3],
1777 plane_mode_angle: 0.0,
1778 }];
1779
1780 super::solve_limited_axes_link_step(super::LimitedAxesLinkStepInput {
1781 local_effector: &local_effector,
1782 local_target: &local_target,
1783 link_index: 0,
1784 base_rotations: &base_rotations,
1785 ik_rotations: &mut ik_rotations,
1786 chain_states: &mut chain_states,
1787 limits,
1788 limit_angle: 0.0,
1789 });
1790
1791 let base_direction = base_rotations[0].mul_vec3a(local_effector).normalize();
1792 let effective = (ik_rotations[0] * base_rotations[0]).normalize();
1793 let stale_direction = limited_direction_without_fixed_axis_working_update(
1794 local_effector,
1795 local_target,
1796 base_rotations[0],
1797 limits,
1798 );
1799 let solved_direction = effective.mul_vec3a(local_effector).normalize();
1800 assert_near(chain_states[0].previous_euler[2], 0.0);
1801 assert!(
1802 (solved_direction - stale_direction).length() > 0.05,
1803 "fixed axis clamp should affect later axis solve; solved={solved_direction:?} stale={stale_direction:?}"
1804 );
1805 assert!(
1806 (solved_direction - local_target).length() < (base_direction - local_target).length(),
1807 "non-identity base should still solve toward target; base={base_direction:?} solved={solved_direction:?} target={local_target:?}"
1808 );
1809 }
1810
1811 fn limited_direction_without_fixed_axis_working_update(
1812 local_effector: Vec3A,
1813 local_target: Vec3A,
1814 base: Quat,
1815 limits: IkAngleLimit,
1816 ) -> Vec3A {
1817 let mut total_euler =
1818 super::decompose_euler_xyz(&super::quat_to_rotation_mat3(base), &[0.0; 3]);
1819 let mut working_effector = local_effector;
1820 let target = local_target.normalize();
1821
1822 for axis_index in [2usize, 1, 0] {
1823 let (lower, upper) = super::limit_axis_bounds(limits, axis_index);
1824 if lower == 0.0 && upper == 0.0 {
1825 total_euler[axis_index] = total_euler[axis_index].clamp(lower, upper);
1826 continue;
1827 }
1828
1829 let axis = super::axis_vec(axis_index);
1830 let signed_angle = super::signed_projected_angle(working_effector, target, axis);
1831 if signed_angle.abs() <= 1.0e-6 {
1832 continue;
1833 }
1834 let next = (total_euler[axis_index] + signed_angle).clamp(lower, upper);
1835 let applied = next - total_euler[axis_index];
1836 total_euler[axis_index] = next;
1837 if applied.abs() > 0.0 {
1838 working_effector =
1839 Quat::from_axis_angle(axis.into(), applied).mul_vec3a(working_effector);
1840 }
1841 }
1842
1843 super::euler_xyz_to_quat(&total_euler)
1844 .normalize()
1845 .mul_vec3a(local_effector)
1846 .normalize()
1847 }
1848
1849 fn legacy_clamp_only_limited_direction(
1850 local_effector: Vec3A,
1851 local_target: Vec3A,
1852 limits: IkAngleLimit,
1853 ) -> Vec3A {
1854 let local_eff_n = local_effector.normalize();
1855 let local_tgt_n = local_target.normalize();
1856 let dot = local_eff_n.dot(local_tgt_n).clamp(-1.0, 1.0);
1857 let angle = dot.acos();
1858 let axis = local_eff_n.cross(local_tgt_n);
1859 let axis_vec = if axis.length() < 1e-5 {
1860 if dot > -1.0 + 1e-5 {
1861 return local_eff_n;
1862 }
1863 let basis = if local_eff_n.x.abs() < 0.9 {
1864 Vec3A::new(1.0, 0.0, 0.0)
1865 } else {
1866 Vec3A::new(0.0, 1.0, 0.0)
1867 };
1868 local_eff_n.cross(basis).normalize()
1869 } else {
1870 axis.normalize()
1871 };
1872 let rotation = Quat::from_axis_angle(axis_vec.into(), angle).normalize();
1873 let euler = super::decompose_euler_xyz(&super::quat_to_rotation_mat3(rotation), &[0.0; 3]);
1874 let clamped = [
1875 euler[0].clamp(limits.min.x, limits.max.x),
1876 euler[1].clamp(limits.min.y, limits.max.y),
1877 euler[2].clamp(limits.min.z, limits.max.z),
1878 ];
1879 super::euler_xyz_to_quat(&clamped)
1880 .normalize()
1881 .mul_vec3a(local_effector)
1882 .normalize()
1883 }
1884
1885 #[test]
1886 fn plane_link_step_matches_saba_total_axis_rotation() {
1887 let base = Quat::from_rotation_x(0.3);
1888 let base_rotations = vec![base];
1889 let mut ik_rotations = vec![Quat::IDENTITY];
1890 let mut chain_states = vec![super::ChainLinkState {
1891 previous_euler: [0.0; 3],
1892 plane_mode_angle: 0.0,
1893 }];
1894 let local_effector = Vec3A::X;
1895 let local_target = Vec3A::Y;
1896
1897 super::solve_plane_link_step(super::PlaneLinkStepInput {
1898 local_effector: &local_effector,
1899 local_target: &local_target,
1900 link_index: 0,
1901 base_rotations: &base_rotations,
1902 ik_rotations: &mut ik_rotations,
1903 chain_states: &mut chain_states,
1904 axis_index: 2,
1905 limits: IkAngleLimit::new(
1906 Vec3A::new(-std::f32::consts::PI, 0.0, -std::f32::consts::PI),
1907 Vec3A::new(std::f32::consts::PI, 0.0, std::f32::consts::PI),
1908 ),
1909 iteration: 0,
1910 limit_angle: 0.0,
1911 });
1912
1913 let effective = (ik_rotations[0] * base_rotations[0]).normalize();
1914 assert_near(
1915 chain_states[0].plane_mode_angle,
1916 std::f32::consts::FRAC_PI_2,
1917 );
1918 assert_vec3a_near(
1919 effective.mul_vec3a(Vec3A::X),
1920 Quat::from_rotation_z(std::f32::consts::FRAC_PI_2).mul_vec3a(Vec3A::X),
1921 );
1922 assert_vec3a_near(effective.mul_vec3a(Vec3A::Z), Vec3A::Z);
1923 }
1924
1925 #[test]
1926 fn append_rotation_propagates_post_ik_link_rotation() {
1927 let model = Arc::new(
1928 ModelArena::new_full(
1929 vec![
1930 BoneInit::new(None, Vec3A::ZERO),
1931 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1932 BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1933 BoneInit::new(None, Vec3A::ZERO),
1934 BoneInit::new(Some(BoneIndex(3)), Vec3A::new(1.0, 0.0, 0.0)),
1935 ],
1936 vec![IkSolverInit {
1937 ik_bone: BoneIndex(2),
1938 target_bone: BoneIndex(1),
1939 links: vec![IkLinkInit::new(BoneIndex(0))],
1940 iteration_count: 1,
1941 limit_angle: 0.0,
1942 }],
1943 vec![AppendTransformInit::new(BoneIndex(3), BoneIndex(0), 1.0).with_rotation()],
1944 )
1945 .unwrap(),
1946 );
1947 let mut runtime = RuntimeInstance::new(model);
1948
1949 runtime.evaluate_current_pose();
1950
1951 assert_vec3a_near(
1952 translation(runtime.world_matrices()[4]),
1953 Vec3A::new(0.0, 1.0, 0.0),
1954 );
1955 }
1956
1957 #[test]
1958 fn append_source_with_own_append_includes_ik_link_rotation() {
1959 let model = Arc::new(
1960 ModelArena::new_full(
1961 vec![
1962 BoneInit::new(None, Vec3A::ZERO),
1963 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
1964 BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
1965 BoneInit::new(None, Vec3A::ZERO),
1966 BoneInit::new(None, Vec3A::ZERO),
1967 BoneInit::new(Some(BoneIndex(4)), Vec3A::new(1.0, 0.0, 0.0)),
1968 ],
1969 vec![IkSolverInit {
1970 ik_bone: BoneIndex(2),
1971 target_bone: BoneIndex(1),
1972 links: vec![IkLinkInit::new(BoneIndex(0))],
1973 iteration_count: 1,
1974 limit_angle: 0.0,
1975 }],
1976 vec![
1977 AppendTransformInit::new(BoneIndex(0), BoneIndex(3), 1.0).with_rotation(),
1978 AppendTransformInit::new(BoneIndex(4), BoneIndex(0), 1.0).with_rotation(),
1979 ],
1980 )
1981 .unwrap(),
1982 );
1983 let mut runtime = RuntimeInstance::new(model);
1984 runtime.pose_mut().set_local_rotation(
1985 BoneIndex(3),
1986 Quat::from_rotation_z(std::f32::consts::FRAC_PI_4),
1987 );
1988
1989 runtime.evaluate_current_pose();
1990
1991 assert_vec3a_near(
1992 translation(runtime.world_matrices()[5]),
1993 Vec3A::new(0.0, 1.0, 0.0),
1994 );
1995 }
1996
1997 #[test]
1998 fn without_ik_evaluation_clears_previous_ik_link_rotation_for_append_sources() {
1999 let model = Arc::new(
2000 ModelArena::new_full(
2001 vec![
2002 BoneInit::new(None, Vec3A::ZERO),
2003 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
2004 BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
2005 BoneInit::new(None, Vec3A::ZERO),
2006 BoneInit::new(None, Vec3A::ZERO),
2007 BoneInit::new(Some(BoneIndex(4)), Vec3A::new(1.0, 0.0, 0.0)),
2008 ],
2009 vec![IkSolverInit {
2010 ik_bone: BoneIndex(2),
2011 target_bone: BoneIndex(1),
2012 links: vec![IkLinkInit::new(BoneIndex(0))],
2013 iteration_count: 1,
2014 limit_angle: 0.0,
2015 }],
2016 vec![
2017 AppendTransformInit::new(BoneIndex(0), BoneIndex(3), 1.0).with_rotation(),
2018 AppendTransformInit::new(BoneIndex(4), BoneIndex(0), 1.0).with_rotation(),
2019 ],
2020 )
2021 .unwrap(),
2022 );
2023 let mut runtime = RuntimeInstance::new(model);
2024 runtime.pose_mut().set_local_rotation(
2025 BoneIndex(3),
2026 Quat::from_rotation_z(std::f32::consts::FRAC_PI_4),
2027 );
2028
2029 runtime.evaluate_current_pose();
2030 runtime.evaluate_current_pose_without_ik();
2031
2032 assert_vec3a_near(
2033 translation(runtime.world_matrices()[5]),
2034 Vec3A::new(
2035 std::f32::consts::FRAC_1_SQRT_2,
2036 std::f32::consts::FRAC_1_SQRT_2,
2037 0.0,
2038 ),
2039 );
2040 }
2041
2042 #[test]
2043 fn shared_ik_link_preserves_accumulated_rotation_for_later_append_source() {
2044 let model = Arc::new(
2045 ModelArena::new_full(
2046 vec![
2047 BoneInit::new(None, Vec3A::ZERO),
2048 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
2049 BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
2050 BoneInit::new(None, Vec3A::ZERO),
2051 BoneInit::new(None, Vec3A::ZERO),
2052 BoneInit::new(Some(BoneIndex(4)), Vec3A::new(1.0, 0.0, 0.0)),
2053 ],
2054 vec![
2055 IkSolverInit {
2056 ik_bone: BoneIndex(2),
2057 target_bone: BoneIndex(1),
2058 links: vec![IkLinkInit::new(BoneIndex(0))],
2059 iteration_count: 1,
2060 limit_angle: 0.0,
2061 },
2062 IkSolverInit {
2063 ik_bone: BoneIndex(2),
2064 target_bone: BoneIndex(2),
2065 links: vec![IkLinkInit::new(BoneIndex(0))],
2066 iteration_count: 1,
2067 limit_angle: 0.0,
2068 },
2069 ],
2070 vec![
2071 AppendTransformInit::new(BoneIndex(0), BoneIndex(3), 1.0).with_rotation(),
2072 AppendTransformInit::new(BoneIndex(4), BoneIndex(0), 1.0).with_rotation(),
2073 ],
2074 )
2075 .unwrap(),
2076 );
2077 let mut runtime = RuntimeInstance::new(model);
2078 runtime.pose_mut().set_local_rotation(
2079 BoneIndex(3),
2080 Quat::from_rotation_z(std::f32::consts::FRAC_PI_4),
2081 );
2082
2083 runtime.evaluate_current_pose();
2084
2085 assert_eq!(runtime.ik_runtime_stats()[1].tolerance_precheck_breaks, 1);
2086 assert_vec3a_near(
2087 translation(runtime.world_matrices()[5]),
2088 Vec3A::new(0.0, 1.0, 0.0),
2089 );
2090 }
2091
2092 #[test]
2093 fn earlier_append_target_updates_after_later_ik_link_rotation() {
2094 let mut append_target = BoneInit::new(None, Vec3A::ZERO);
2095 append_target.transform_order = 0;
2096 let mut append_child = BoneInit::new(Some(BoneIndex(3)), Vec3A::X);
2097 append_child.transform_order = 1;
2098 let mut link = BoneInit::new(None, Vec3A::ZERO);
2099 link.transform_order = 10;
2100 let mut effector = BoneInit::new(Some(BoneIndex(0)), Vec3A::X);
2101 effector.transform_order = 11;
2102 let mut controller = BoneInit::new(None, Vec3A::Y);
2103 controller.transform_order = 12;
2104
2105 let model = Arc::new(
2106 ModelArena::new_full(
2107 vec![link, effector, controller, append_target, append_child],
2108 vec![IkSolverInit {
2109 ik_bone: BoneIndex(2),
2110 target_bone: BoneIndex(1),
2111 links: vec![IkLinkInit::new(BoneIndex(0))],
2112 iteration_count: 1,
2113 limit_angle: 0.0,
2114 }],
2115 vec![AppendTransformInit::new(BoneIndex(3), BoneIndex(0), 1.0).with_rotation()],
2116 )
2117 .unwrap(),
2118 );
2119 let mut runtime = RuntimeInstance::new(model);
2120
2121 runtime.evaluate_current_pose();
2122
2123 assert_vec3a_near(
2124 translation(runtime.world_matrices()[4]),
2125 Vec3A::new(0.0, 1.0, 0.0),
2126 );
2127 }
2128
2129 #[test]
2130 fn earlier_append_target_preserves_later_ik_link_source_append_rotation() {
2131 let mut append_target = BoneInit::new(None, Vec3A::ZERO);
2132 append_target.transform_order = 0;
2133 let mut append_child = BoneInit::new(Some(BoneIndex(3)), Vec3A::X);
2134 append_child.transform_order = 1;
2135 let mut append_driver = BoneInit::new(None, Vec3A::ZERO);
2136 append_driver.transform_order = 9;
2137 let mut link = BoneInit::new(None, Vec3A::ZERO);
2138 link.transform_order = 10;
2139 let mut effector = BoneInit::new(Some(BoneIndex(0)), Vec3A::X);
2140 effector.transform_order = 11;
2141 let mut controller = BoneInit::new(None, Vec3A::Y);
2142 controller.transform_order = 12;
2143
2144 let model = Arc::new(
2145 ModelArena::new_full(
2146 vec![
2147 link,
2148 effector,
2149 controller,
2150 append_target,
2151 append_child,
2152 append_driver,
2153 ],
2154 vec![IkSolverInit {
2155 ik_bone: BoneIndex(2),
2156 target_bone: BoneIndex(1),
2157 links: vec![IkLinkInit::new(BoneIndex(0))],
2158 iteration_count: 1,
2159 limit_angle: 0.0,
2160 }],
2161 vec![
2162 AppendTransformInit::new(BoneIndex(0), BoneIndex(5), 1.0).with_rotation(),
2163 AppendTransformInit::new(BoneIndex(3), BoneIndex(0), 1.0).with_rotation(),
2164 ],
2165 )
2166 .unwrap(),
2167 );
2168 let mut runtime = RuntimeInstance::new(model);
2169 runtime.pose_mut().set_local_rotation(
2170 BoneIndex(5),
2171 Quat::from_rotation_z(std::f32::consts::FRAC_PI_4),
2172 );
2173
2174 runtime.evaluate_current_pose();
2175
2176 assert_vec3a_near(
2177 translation(runtime.world_matrices()[4]),
2178 Vec3A::new(0.0, 1.0, 0.0),
2179 );
2180 }
2181
2182 #[test]
2183 fn transitive_append_target_recomputes_after_opposite_phase_ik_source_rotation() {
2184 let mut append_a = BoneInit::new(None, Vec3A::ZERO);
2185 append_a.transform_order = 0;
2186 let mut append_b = BoneInit::new(None, Vec3A::ZERO);
2187 append_b.transform_order = 1;
2188 let mut append_b_child = BoneInit::new(Some(BoneIndex(4)), Vec3A::X);
2189 append_b_child.transform_order = 2;
2190
2191 let mut link = BoneInit::new(None, Vec3A::ZERO);
2192 link.transform_order = 10;
2193 link.transform_after_physics = true;
2194 let mut effector = BoneInit::new(Some(BoneIndex(0)), Vec3A::X);
2195 effector.transform_order = 11;
2196 effector.transform_after_physics = true;
2197 let mut controller = BoneInit::new(None, Vec3A::Y);
2198 controller.transform_order = 12;
2199 controller.transform_after_physics = true;
2200
2201 let model = Arc::new(
2202 ModelArena::new_full(
2203 vec![
2204 link,
2205 effector,
2206 controller,
2207 append_a,
2208 append_b,
2209 append_b_child,
2210 ],
2211 vec![IkSolverInit {
2212 ik_bone: BoneIndex(2),
2213 target_bone: BoneIndex(1),
2214 links: vec![IkLinkInit::new(BoneIndex(0))],
2215 iteration_count: 1,
2216 limit_angle: 0.0,
2217 }],
2218 vec![
2219 AppendTransformInit::new(BoneIndex(3), BoneIndex(0), 1.0).with_rotation(),
2220 AppendTransformInit::new(BoneIndex(4), BoneIndex(3), 1.0).with_rotation(),
2221 ],
2222 )
2223 .unwrap(),
2224 );
2225 let mut runtime = RuntimeInstance::new(model);
2226
2227 runtime.evaluate_current_pose();
2228
2229 assert_vec3a_near(
2230 translation(runtime.world_matrices()[5]),
2231 Vec3A::new(0.0, 1.0, 0.0),
2232 );
2233 }
2234
2235 #[test]
2236 fn mixed_phase_ik_updates_opposite_phase_controller_dependency() {
2237 let mut link_a = BoneInit::new(None, Vec3A::ZERO);
2238 link_a.transform_order = 0;
2239 let mut effector_a = BoneInit::new(Some(BoneIndex(0)), Vec3A::X);
2240 effector_a.transform_order = 1;
2241 let mut controller_a = BoneInit::new(None, Vec3A::Y);
2242 controller_a.transform_order = 2;
2243 let mut after_append = BoneInit::new(None, Vec3A::ZERO);
2244 after_append.transform_order = 3;
2245 after_append.transform_after_physics = true;
2246 let mut link_b = BoneInit::new(None, Vec3A::ZERO);
2247 link_b.transform_order = 4;
2248 let mut effector_b = BoneInit::new(Some(BoneIndex(4)), Vec3A::X);
2249 effector_b.transform_order = 5;
2250 let mut controller_b = BoneInit::new(Some(BoneIndex(3)), Vec3A::X);
2251 controller_b.transform_order = 6;
2252
2253 let model = Arc::new(
2254 ModelArena::new_full(
2255 vec![
2256 link_a,
2257 effector_a,
2258 controller_a,
2259 after_append,
2260 link_b,
2261 effector_b,
2262 controller_b,
2263 ],
2264 vec![
2265 IkSolverInit {
2266 ik_bone: BoneIndex(2),
2267 target_bone: BoneIndex(1),
2268 links: vec![IkLinkInit::new(BoneIndex(0))],
2269 iteration_count: 1,
2270 limit_angle: 0.0,
2271 },
2272 IkSolverInit {
2273 ik_bone: BoneIndex(6),
2274 target_bone: BoneIndex(5),
2275 links: vec![IkLinkInit::new(BoneIndex(4))],
2276 iteration_count: 1,
2277 limit_angle: 0.0,
2278 },
2279 ],
2280 vec![AppendTransformInit::new(BoneIndex(3), BoneIndex(0), 1.0).with_rotation()],
2281 )
2282 .unwrap(),
2283 );
2284 let mut runtime = RuntimeInstance::new(model);
2285
2286 runtime.evaluate_current_pose();
2287
2288 assert_vec3a_near(
2289 translation(runtime.world_matrices()[5]),
2290 Vec3A::new(0.0, 1.0, 0.0),
2291 );
2292 assert_vec3a_near(
2293 translation(runtime.world_matrices()[6]),
2294 Vec3A::new(0.0, 1.0, 0.0),
2295 );
2296 assert_vec3a_near(
2297 translation(runtime.world_matrices()[3]),
2298 Vec3A::new(0.0, 0.0, 0.0),
2299 );
2300 }
2301
2302 #[test]
2303 fn after_physics_plain_child_recomputes_after_pre_physics_parent_ik() {
2304 let mut after_child = BoneInit::new(Some(BoneIndex(0)), Vec3A::X);
2305 after_child.transform_order = 3;
2306 after_child.transform_after_physics = true;
2307
2308 let model = Arc::new(
2309 ModelArena::new_with_ik(
2310 vec![
2311 BoneInit::new(None, Vec3A::ZERO),
2312 BoneInit::new(Some(BoneIndex(0)), Vec3A::X),
2313 BoneInit::new(None, Vec3A::Y),
2314 after_child,
2315 ],
2316 vec![IkSolverInit {
2317 ik_bone: BoneIndex(2),
2318 target_bone: BoneIndex(1),
2319 links: vec![IkLinkInit::new(BoneIndex(0))],
2320 iteration_count: 1,
2321 limit_angle: 0.0,
2322 }],
2323 )
2324 .unwrap(),
2325 );
2326 let mut runtime = RuntimeInstance::new(model);
2327
2328 runtime.evaluate_current_pose();
2329
2330 assert_vec3a_near(
2331 translation(runtime.world_matrices()[3]),
2332 Vec3A::new(0.0, 1.0, 0.0),
2333 );
2334 }
2335
2336 #[test]
2337 fn pre_physics_child_recomputes_after_after_physics_append_parent() {
2338 let mut after_parent = BoneInit::new(None, Vec3A::ZERO);
2339 after_parent.transform_order = 1;
2340 after_parent.transform_after_physics = true;
2341 let mut pre_child = BoneInit::new(Some(BoneIndex(1)), Vec3A::X);
2342 pre_child.transform_order = 2;
2343
2344 let model = Arc::new(
2345 ModelArena::new_full(
2346 vec![BoneInit::new(None, Vec3A::ZERO), after_parent, pre_child],
2347 Vec::new(),
2348 vec![AppendTransformInit::new(BoneIndex(1), BoneIndex(0), 1.0).with_rotation()],
2349 )
2350 .unwrap(),
2351 );
2352 let mut runtime = RuntimeInstance::new(model);
2353 runtime.pose_mut().set_local_rotation(
2354 BoneIndex(0),
2355 Quat::from_rotation_z(std::f32::consts::FRAC_PI_2),
2356 );
2357
2358 runtime.evaluate_current_pose();
2359
2360 assert_vec3a_near(
2361 translation(runtime.world_matrices()[2]),
2362 Vec3A::new(0.0, 1.0, 0.0),
2363 );
2364 }
2365
2366 #[test]
2367 fn append_target_recomputes_after_opposite_phase_ik_source_rotation() {
2368 let mut after_controller = BoneInit::new(None, Vec3A::Y);
2369 after_controller.transform_order = 2;
2370 after_controller.transform_after_physics = true;
2371 let mut append_target = BoneInit::new(None, Vec3A::ZERO);
2372 append_target.transform_order = 3;
2373 let append_child = BoneInit::new(Some(BoneIndex(3)), Vec3A::X);
2374
2375 let model = Arc::new(
2376 ModelArena::new_full(
2377 vec![
2378 BoneInit::new(None, Vec3A::ZERO),
2379 BoneInit::new(Some(BoneIndex(0)), Vec3A::X),
2380 after_controller,
2381 append_target,
2382 append_child,
2383 ],
2384 vec![IkSolverInit {
2385 ik_bone: BoneIndex(2),
2386 target_bone: BoneIndex(1),
2387 links: vec![IkLinkInit::new(BoneIndex(0))],
2388 iteration_count: 1,
2389 limit_angle: 0.0,
2390 }],
2391 vec![AppendTransformInit::new(BoneIndex(3), BoneIndex(0), 1.0).with_rotation()],
2392 )
2393 .unwrap(),
2394 );
2395 let mut runtime = RuntimeInstance::new(model);
2396
2397 runtime.evaluate_current_pose();
2398
2399 assert_vec3a_near(
2400 translation(runtime.world_matrices()[4]),
2401 Vec3A::new(0.0, 1.0, 0.0),
2402 );
2403 }
2404
2405 #[test]
2406 fn scratch_ik_capacities_stable_after_repeated_evaluate() {
2407 let model = Arc::new(
2408 ModelArena::new_with_ik(
2409 vec![
2410 BoneInit::new(None, Vec3A::ZERO),
2411 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(1.0, 0.0, 0.0)),
2412 BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
2413 BoneInit::new(None, Vec3A::ZERO),
2414 BoneInit::new(Some(BoneIndex(3)), Vec3A::new(1.0, 0.0, 0.0)),
2415 BoneInit::new(Some(BoneIndex(4)), Vec3A::new(1.0, 0.0, 0.0)),
2416 BoneInit::new(None, Vec3A::new(0.0, 1.0, 0.0)),
2417 ],
2418 vec![
2419 IkSolverInit {
2420 ik_bone: BoneIndex(2),
2421 target_bone: BoneIndex(1),
2422 links: vec![IkLinkInit::new(BoneIndex(0))],
2423 iteration_count: 1,
2424 limit_angle: 0.0,
2425 },
2426 IkSolverInit {
2427 ik_bone: BoneIndex(6),
2428 target_bone: BoneIndex(5),
2429 links: vec![IkLinkInit::new(BoneIndex(3)), IkLinkInit::new(BoneIndex(4))],
2430 iteration_count: 1,
2431 limit_angle: 0.0,
2432 },
2433 ],
2434 )
2435 .unwrap(),
2436 );
2437 let mut runtime = RuntimeInstance::new(model);
2438
2439 runtime.evaluate_current_pose();
2440
2441 let cap_links = runtime.ik_scratch.links.capacity();
2442 let cap_base = runtime.ik_scratch.base_rotations.capacity();
2443 let cap_base_ik = runtime.ik_scratch.base_ik_rotations.capacity();
2444 let cap_ik = runtime.ik_scratch.ik_rotations.capacity();
2445 let cap_best = runtime.ik_scratch.best_ik_rotations.capacity();
2446 let cap_chain = runtime.ik_scratch.chain_states.capacity();
2447
2448 for _ in 0..10 {
2449 runtime.evaluate_current_pose();
2450 }
2451
2452 assert_eq!(runtime.ik_scratch.links.capacity(), cap_links);
2453 assert_eq!(runtime.ik_scratch.base_rotations.capacity(), cap_base);
2454 assert_eq!(runtime.ik_scratch.base_ik_rotations.capacity(), cap_base_ik);
2455 assert_eq!(runtime.ik_scratch.ik_rotations.capacity(), cap_ik);
2456 assert_eq!(runtime.ik_scratch.best_ik_rotations.capacity(), cap_best);
2457 assert_eq!(runtime.ik_scratch.chain_states.capacity(), cap_chain);
2458 }
2459
2460 #[test]
2461 fn scratch_morph_capacity_stable_after_repeated_clip_frame() {
2462 let model = Arc::new(
2463 ModelArena::new_with_morphs(
2464 vec![
2465 BoneInit::new(None, Vec3A::ZERO),
2466 BoneInit::new(Some(BoneIndex(0)), Vec3A::new(0.0, 1.0, 0.0)),
2467 ],
2468 Vec::new(),
2469 Vec::new(),
2470 crate::MorphInit {
2471 morph_count: 2,
2472 bone_offsets: vec![crate::BoneMorphOffset {
2473 target_bone: BoneIndex(1),
2474 position_offset: Vec3A::new(0.0, 0.0, 2.0),
2475 rotation_offset: Quat::IDENTITY,
2476 }],
2477 bone_spans: vec![
2478 crate::MorphOffsetSpan { start: 0, count: 1 },
2479 crate::MorphOffsetSpan::default(),
2480 ],
2481 group_offsets: vec![crate::GroupMorphOffset {
2482 child_morph: crate::MorphIndex(0),
2483 ratio: 0.5,
2484 }],
2485 group_spans: vec![
2486 crate::MorphOffsetSpan::default(),
2487 crate::MorphOffsetSpan { start: 0, count: 1 },
2488 ],
2489 ..crate::MorphInit::default()
2490 },
2491 )
2492 .unwrap(),
2493 );
2494 let clip = AnimationClip::new_with_morphs(
2495 Vec::new(),
2496 vec![crate::MorphAnimationBinding {
2497 morph: crate::MorphIndex(1),
2498 track: crate::MorphTrack::from_keyframes(vec![
2499 crate::MorphKeyframe::new(0, 0.0),
2500 crate::MorphKeyframe::new(10, 1.0),
2501 ]),
2502 }],
2503 );
2504 let mut runtime = RuntimeInstance::new_with_morph_count(model, 2);
2505
2506 runtime.evaluate_clip_frame(&clip, 5.0);
2507
2508 let cap_expanded = runtime.morph_scratch.expanded_weights.capacity();
2509
2510 for _ in 0..10 {
2511 runtime.evaluate_clip_frame(&clip, 5.0);
2512 }
2513
2514 assert_eq!(
2515 runtime.morph_scratch.expanded_weights.capacity(),
2516 cap_expanded
2517 );
2518 }
2519}