1use glam::{Mat4, Quat, Vec3A};
2
3use crate::IkAngleLimit;
4
5#[derive(Clone, Debug, PartialEq)]
6pub struct IkChainLinkDefinition {
7 pub bone_slot: usize,
8 pub angle_limit: Option<IkAngleLimit>,
9}
10
11#[derive(Clone, Debug, PartialEq)]
12pub struct IkChainDefinition {
13 pub parent_slots: Vec<Option<usize>>,
14 pub rest_positions: Vec<Vec3A>,
15 pub fixed_axes: Vec<Option<Vec3A>>,
16 pub target_slot: usize,
17 pub links: Vec<IkChainLinkDefinition>,
18 pub iteration_count: u32,
19 pub limit_angle: f32,
20}
21
22#[derive(Clone, Copy, Debug, PartialEq)]
23pub struct IkChainPoseInput<'a> {
24 pub parent_world_matrix: Option<Mat4>,
25 pub local_position_offsets: &'a [Vec3A],
26 pub local_rotations: &'a [Quat],
27 pub goal_position: Vec3A,
28 pub tolerance: f32,
29 pub max_iterations_cap: Option<u32>,
30}
31
32#[derive(Clone, Debug, PartialEq)]
33pub struct IkChainSolveOutput {
34 pub solved_link_rotations: Vec<Quat>,
35 pub final_distance: f32,
36 pub executed_iterations: u32,
37 pub link_steps: u32,
38}
39
40#[derive(Debug)]
41pub struct IkChainSolver {
42 definition: IkChainDefinition,
43 world_matrices: Vec<Mat4>,
44 local_rotations: Vec<Quat>,
45 base_rotations: Vec<Quat>,
46 ik_rotations: Vec<Quat>,
47 best_ik_rotations: Vec<Quat>,
48 chain_states: Vec<ChainLinkState>,
49}
50
51impl IkChainSolver {
52 pub fn new(definition: IkChainDefinition) -> Self {
53 let bone_count = definition.rest_positions.len();
54 let link_count = definition.links.len();
55 Self {
56 definition,
57 world_matrices: vec![Mat4::IDENTITY; bone_count],
58 local_rotations: vec![Quat::IDENTITY; bone_count],
59 base_rotations: Vec::with_capacity(link_count),
60 ik_rotations: Vec::with_capacity(link_count),
61 best_ik_rotations: Vec::with_capacity(link_count),
62 chain_states: Vec::with_capacity(link_count),
63 }
64 }
65
66 pub fn solve(&mut self, input: IkChainPoseInput<'_>) -> IkChainSolveOutput {
67 let tolerance = input.tolerance.max(0.0);
68 let iteration_count = input
69 .max_iterations_cap
70 .map(|cap| self.definition.iteration_count.min(cap))
71 .unwrap_or(self.definition.iteration_count)
72 .max(1) as usize;
73 let limit_angle = self.definition.limit_angle.max(0.0);
74 let link_count = self.definition.links.len();
75
76 self.local_rotations.copy_from_slice(input.local_rotations);
77 self.update_world_matrices(input);
78
79 self.base_rotations.clear();
80 self.base_rotations.extend(
81 self.definition
82 .links
83 .iter()
84 .map(|link| self.local_rotations[link.bone_slot]),
85 );
86 self.ik_rotations.clear();
87 self.ik_rotations.resize(link_count, Quat::IDENTITY);
88 self.best_ik_rotations.clear();
89 self.best_ik_rotations.resize(link_count, Quat::IDENTITY);
90 self.chain_states.clear();
91 self.chain_states
92 .resize_with(link_count, ChainLinkState::default);
93
94 self.apply_link_rotations();
95 self.update_world_matrices(input);
96
97 let mut final_distance = f32::MAX;
98 let mut best_distance = f32::MAX;
99 let mut executed_iterations = 0u32;
100 let mut link_steps = 0u32;
101
102 for iteration in 0..iteration_count {
103 let eff_pos = translation(self.world_matrices[self.definition.target_slot]);
104 final_distance = (eff_pos - input.goal_position).length();
105 if final_distance <= tolerance {
106 break;
107 }
108 executed_iterations += 1;
109
110 for link_index in 0..link_count {
111 let link = &self.definition.links[link_index];
112 let link_slot = link.bone_slot;
113
114 if link_slot == self.definition.target_slot {
115 continue;
116 }
117
118 let link_world = self.world_matrices[link_slot];
119 let link_pos = translation(link_world);
120 let eff_pos = translation(self.world_matrices[self.definition.target_slot]);
121 let link_world_rot = rotation(link_world);
122 let local_effector = link_world_rot.inverse().mul_vec3a(eff_pos - link_pos);
123 let local_target = link_world_rot
124 .inverse()
125 .mul_vec3a(input.goal_position - link_pos);
126
127 if local_effector.length_squared() <= f32::EPSILON
128 || local_target.length_squared() <= f32::EPSILON
129 {
130 continue;
131 }
132
133 solve_link_step(LinkStepInput {
134 local_effector: &local_effector,
135 local_target: &local_target,
136 link_index,
137 base_rotations: &self.base_rotations,
138 ik_rotations: &mut self.ik_rotations,
139 chain_states: &mut self.chain_states,
140 angle_limit: link.angle_limit,
141 iteration,
142 limit_angle,
143 });
144
145 self.apply_link_rotations();
146 self.update_world_matrices(input);
147 link_steps += 1;
148 }
149
150 let current_distance = {
151 let eff = translation(self.world_matrices[self.definition.target_slot]);
152 (eff - input.goal_position).length()
153 };
154 final_distance = current_distance;
155 if current_distance < best_distance {
156 best_distance = current_distance;
157 self.best_ik_rotations.copy_from_slice(&self.ik_rotations);
158 if current_distance <= tolerance {
159 break;
160 }
161 } else {
162 self.ik_rotations.copy_from_slice(&self.best_ik_rotations);
163 self.apply_link_rotations();
164 self.update_world_matrices(input);
165 break;
166 }
167 }
168
169 self.ik_rotations.copy_from_slice(&self.best_ik_rotations);
170 self.apply_link_rotations();
171 self.update_world_matrices(input);
172
173 let solved_link_rotations = self
174 .definition
175 .links
176 .iter()
177 .map(|link| self.local_rotations[link.bone_slot])
178 .collect();
179
180 IkChainSolveOutput {
181 solved_link_rotations,
182 final_distance,
183 executed_iterations,
184 link_steps,
185 }
186 }
187
188 pub(crate) fn update_world_matrices(&mut self, input: IkChainPoseInput<'_>) {
189 update_mini_chain_world_matrices(
190 &self.definition,
191 input.parent_world_matrix.unwrap_or(Mat4::IDENTITY),
192 input.local_position_offsets,
193 &self.local_rotations,
194 &mut self.world_matrices,
195 );
196 }
197
198 fn apply_link_rotations(&mut self) {
199 for (i, link) in self.definition.links.iter().enumerate() {
200 let effective = (self.ik_rotations[i] * self.base_rotations[i]).normalize();
201 self.local_rotations[link.bone_slot] = constrain_rotation_to_axis_if_needed(
202 effective,
203 self.definition.fixed_axes[link.bone_slot],
204 );
205 }
206 }
207}
208
209pub(crate) fn update_mini_chain_world_matrices(
210 definition: &IkChainDefinition,
211 parent_world_matrix: Mat4,
212 local_position_offsets: &[Vec3A],
213 local_rotations: &[Quat],
214 world_matrices: &mut [Mat4],
215) {
216 for slot in 0..definition.rest_positions.len() {
217 let local_position = definition.rest_positions[slot] + local_position_offsets[slot];
218 let local_rotation = constrain_rotation_to_axis_if_needed(
219 local_rotations[slot],
220 definition.fixed_axes[slot],
221 );
222 let local_matrix = Mat4::from_scale_rotation_translation(
223 Vec3A::ONE.into(),
224 local_rotation,
225 local_position.into(),
226 );
227 world_matrices[slot] = match definition.parent_slots[slot] {
228 Some(parent) => world_matrices[parent] * local_matrix,
229 None => parent_world_matrix * local_matrix,
230 };
231 }
232}
233
234pub(crate) fn solve_link_step(input: LinkStepInput<'_>) {
235 let single_axis = get_single_axis_limit(input.angle_limit);
236 if let (Some(angle_limit), Some(axis_index)) = (input.angle_limit, single_axis) {
237 solve_plane_link_step(PlaneLinkStepInput {
238 local_effector: input.local_effector,
239 local_target: input.local_target,
240 link_index: input.link_index,
241 base_rotations: input.base_rotations,
242 ik_rotations: input.ik_rotations,
243 chain_states: input.chain_states,
244 axis_index,
245 limits: angle_limit,
246 iteration: input.iteration,
247 limit_angle: input.limit_angle,
248 });
249 } else if let Some(angle_limit) = input.angle_limit {
250 solve_limited_axes_link_step(LimitedAxesLinkStepInput {
251 local_effector: input.local_effector,
252 local_target: input.local_target,
253 link_index: input.link_index,
254 base_rotations: input.base_rotations,
255 ik_rotations: input.ik_rotations,
256 chain_states: input.chain_states,
257 limits: angle_limit,
258 limit_angle: input.limit_angle,
259 });
260 } else {
261 solve_unconstrained_link_step(UnconstrainedLinkStepInput {
262 local_effector: input.local_effector,
263 local_target: input.local_target,
264 link_index: input.link_index,
265 base_rotations: input.base_rotations,
266 ik_rotations: input.ik_rotations,
267 limit_angle: input.limit_angle,
268 });
269 }
270}
271
272pub(crate) struct LinkStepInput<'a> {
273 pub local_effector: &'a Vec3A,
274 pub local_target: &'a Vec3A,
275 pub link_index: usize,
276 pub base_rotations: &'a [Quat],
277 pub ik_rotations: &'a mut [Quat],
278 pub chain_states: &'a mut [ChainLinkState],
279 pub angle_limit: Option<IkAngleLimit>,
280 pub iteration: usize,
281 pub limit_angle: f32,
282}
283
284struct UnconstrainedLinkStepInput<'a> {
285 local_effector: &'a Vec3A,
286 local_target: &'a Vec3A,
287 link_index: usize,
288 base_rotations: &'a [Quat],
289 ik_rotations: &'a mut [Quat],
290 limit_angle: f32,
291}
292
293fn solve_unconstrained_link_step(input: UnconstrainedLinkStepInput<'_>) {
294 let local_eff_n = input.local_effector.normalize();
295 let local_tgt_n = input.local_target.normalize();
296 let dot = local_eff_n.dot(local_tgt_n).clamp(-1.0, 1.0);
297 let mut angle = dot.acos();
298
299 let tiny_angle = 1e-3 * std::f32::consts::PI / 180.0;
300 if angle < tiny_angle {
301 return;
302 }
303
304 if input.limit_angle > 0.0 {
305 angle = angle.min(input.limit_angle);
306 }
307
308 let axis = local_eff_n.cross(local_tgt_n);
309 let axis_vec = if axis.length() < 1e-5 {
310 if dot > -1.0 + 1e-5 {
311 return;
312 }
313 let basis = if local_eff_n.x.abs() < 0.9 {
314 Vec3A::new(1.0, 0.0, 0.0)
315 } else {
316 Vec3A::new(0.0, 1.0, 0.0)
317 };
318 local_eff_n.cross(basis).normalize()
319 } else {
320 axis.normalize()
321 };
322
323 let delta = Quat::from_axis_angle(axis_vec.into(), angle);
324 let base = input.base_rotations[input.link_index];
325 let ik = input.ik_rotations[input.link_index];
326 let chain_rotation = (ik * base * delta).normalize();
327
328 input.ik_rotations[input.link_index] = (chain_rotation * base.inverse()).normalize();
329}
330
331pub(crate) fn translation(matrix: Mat4) -> Vec3A {
332 Vec3A::from_vec4(matrix.w_axis)
333}
334
335pub(crate) fn rotation(matrix: Mat4) -> Quat {
336 matrix.to_scale_rotation_translation().1
337}
338
339pub(crate) fn constrain_rotation_to_axis(rotation: Quat, axis: Vec3A) -> Quat {
340 let axis = axis.normalize();
341 let vector = Vec3A::new(rotation.x, rotation.y, rotation.z);
342 let projected = axis * vector.dot(axis);
343 let twist = Quat::from_xyzw(projected.x, projected.y, projected.z, rotation.w);
344 if twist.length_squared() <= f32::EPSILON {
345 Quat::IDENTITY
346 } else {
347 twist.normalize()
348 }
349}
350
351fn constrain_rotation_to_axis_if_needed(rotation: Quat, axis: Option<Vec3A>) -> Quat {
352 axis.map_or(rotation, |axis| constrain_rotation_to_axis(rotation, axis))
353}
354
355#[derive(Clone, Copy, Debug, Default, PartialEq)]
356pub(crate) struct ChainLinkState {
357 pub previous_euler: [f32; 3],
358 pub plane_mode_angle: f32,
359}
360
361pub(crate) fn get_single_axis_limit(limit: Option<IkAngleLimit>) -> Option<usize> {
362 let limit = limit?;
363 let has = [
364 limit.min.x != 0.0 || limit.max.x != 0.0,
365 limit.min.y != 0.0 || limit.max.y != 0.0,
366 limit.min.z != 0.0 || limit.max.z != 0.0,
367 ];
368 if has[0]
369 && limit.min.y == 0.0
370 && limit.max.y == 0.0
371 && limit.min.z == 0.0
372 && limit.max.z == 0.0
373 {
374 return Some(0);
375 }
376 if has[1]
377 && limit.min.x == 0.0
378 && limit.max.x == 0.0
379 && limit.min.z == 0.0
380 && limit.max.z == 0.0
381 {
382 return Some(1);
383 }
384 if has[2]
385 && limit.min.x == 0.0
386 && limit.max.x == 0.0
387 && limit.min.y == 0.0
388 && limit.max.y == 0.0
389 {
390 return Some(2);
391 }
392 None
393}
394
395pub(crate) fn quat_to_rotation_mat3(rotation: Quat) -> [f32; 9] {
396 let [x, y, z, w] = rotation.normalize().to_array();
397 let x2 = x + x;
398 let y2 = y + y;
399 let z2 = z + z;
400 let xx = x * x2;
401 let xy = x * y2;
402 let xz = x * z2;
403 let yy = y * y2;
404 let yz = y * z2;
405 let zz = z * z2;
406 let wx = w * x2;
407 let wy = w * y2;
408 let wz = w * z2;
409 [
410 1.0 - (yy + zz),
411 xy + wz,
412 xz - wy,
413 xy - wz,
414 1.0 - (xx + zz),
415 yz + wx,
416 xz + wy,
417 yz - wx,
418 1.0 - (xx + yy),
419 ]
420}
421
422pub(crate) fn decompose_euler_xyz(mat: &[f32; 9], before: &[f32; 3]) -> [f32; 3] {
423 let sy = -mat[2];
424 let mut result: [f32; 3];
425 if 1.0 - sy.abs() < 1e-6 {
426 let y = sy.asin();
427 let sx = before[0].sin();
428 let sz = before[2].sin();
429 if sx.abs() < sz.abs() {
430 let cx = before[0].cos();
431 result = if cx > 0.0 {
432 [0.0, y, (-mat[3]).asin()]
433 } else {
434 [std::f32::consts::PI, y, mat[3].asin()]
435 };
436 } else {
437 let cz = before[2].cos();
438 result = if cz > 0.0 {
439 [(-mat[7]).asin(), y, 0.0]
440 } else {
441 [mat[7].asin(), y, std::f32::consts::PI]
442 };
443 }
444 } else {
445 result = [mat[5].atan2(mat[8]), (-mat[2]).asin(), mat[1].atan2(mat[0])];
446 }
447
448 let pi = std::f32::consts::PI;
449 let candidates: [[f32; 3]; 8] = [
450 [result[0] + pi, pi - result[1], result[2] + pi],
451 [result[0] + pi, pi - result[1], result[2] - pi],
452 [result[0] + pi, -pi - result[1], result[2] + pi],
453 [result[0] + pi, -pi - result[1], result[2] - pi],
454 [result[0] - pi, pi - result[1], result[2] + pi],
455 [result[0] - pi, pi - result[1], result[2] - pi],
456 [result[0] - pi, -pi - result[1], result[2] + pi],
457 [result[0] - pi, -pi - result[1], result[2] - pi],
458 ];
459 let mut min_error = diff_angle(result[0], before[0]).abs()
460 + diff_angle(result[1], before[1]).abs()
461 + diff_angle(result[2], before[2]).abs();
462 for candidate in &candidates {
463 let error = diff_angle(candidate[0], before[0]).abs()
464 + diff_angle(candidate[1], before[1]).abs()
465 + diff_angle(candidate[2], before[2]).abs();
466 if error < min_error {
467 min_error = error;
468 result = *candidate;
469 }
470 }
471 result
472}
473
474fn diff_angle(a: f32, b: f32) -> f32 {
475 let diff = normalize_angle(a) - normalize_angle(b);
476 if diff > std::f32::consts::PI {
477 diff - std::f32::consts::TAU
478 } else if diff < -std::f32::consts::PI {
479 diff + std::f32::consts::TAU
480 } else {
481 diff
482 }
483}
484
485fn normalize_angle(angle: f32) -> f32 {
486 let mut result = angle;
487 while result >= std::f32::consts::TAU {
488 result -= std::f32::consts::TAU;
489 }
490 while result < 0.0 {
491 result += std::f32::consts::TAU;
492 }
493 result
494}
495
496pub(crate) fn euler_xyz_to_quat(euler: &[f32; 3]) -> Quat {
497 let [x, y, z] = *euler;
498 let c1 = (x / 2.0).cos();
499 let c2 = (y / 2.0).cos();
500 let c3 = (z / 2.0).cos();
501 let s1 = (x / 2.0).sin();
502 let s2 = (y / 2.0).sin();
503 let s3 = (z / 2.0).sin();
504 Quat::from_xyzw(
505 s1 * c2 * c3 + c1 * s2 * s3,
506 c1 * s2 * c3 - s1 * c2 * s3,
507 c1 * c2 * s3 + s1 * s2 * c3,
508 c1 * c2 * c3 - s1 * s2 * s3,
509 )
510}
511
512pub(crate) struct LimitedAxesLinkStepInput<'a> {
513 pub local_effector: &'a Vec3A,
514 pub local_target: &'a Vec3A,
515 pub link_index: usize,
516 pub base_rotations: &'a [Quat],
517 pub ik_rotations: &'a mut [Quat],
518 pub chain_states: &'a mut [ChainLinkState],
519 pub limits: IkAngleLimit,
520 pub limit_angle: f32,
521}
522
523pub(crate) fn solve_limited_axes_link_step(input: LimitedAxesLinkStepInput<'_>) {
524 let state = &mut input.chain_states[input.link_index];
525 let base = input.base_rotations[input.link_index];
526 let current = (input.ik_rotations[input.link_index] * base).normalize();
527 let current_mat = quat_to_rotation_mat3(current);
528 let mut total_euler = decompose_euler_xyz(¤t_mat, &state.previous_euler);
529 let mut working_effector = *input.local_effector;
530 let target = input.local_target.normalize();
531
532 for axis_index in [2usize, 1, 0] {
536 let (lower, upper) = limit_axis_bounds(input.limits, axis_index);
537 if lower == 0.0 && upper == 0.0 {
538 let next = total_euler[axis_index].clamp(lower, upper);
539 let applied = next - total_euler[axis_index];
540 total_euler[axis_index] = next;
541 if applied.abs() > 0.0 {
542 working_effector = Quat::from_axis_angle(axis_vec(axis_index).into(), applied)
543 .mul_vec3a(working_effector);
544 }
545 continue;
546 }
547
548 let axis = axis_vec(axis_index);
549 let signed_angle = signed_projected_angle(working_effector, target, axis);
550 if signed_angle.abs() <= 1.0e-6 {
551 continue;
552 }
553 let step = if input.limit_angle > 0.0 {
554 signed_angle.clamp(-input.limit_angle, input.limit_angle)
555 } else {
556 signed_angle
557 };
558 let next = (total_euler[axis_index] + step).clamp(lower, upper);
559 let applied = next - total_euler[axis_index];
560 total_euler[axis_index] = next;
561 if applied.abs() > 0.0 {
562 working_effector =
563 Quat::from_axis_angle(axis.into(), applied).mul_vec3a(working_effector);
564 }
565 }
566
567 state.previous_euler = total_euler;
568 let chain_rotation = euler_xyz_to_quat(&total_euler).normalize();
569 input.ik_rotations[input.link_index] = (chain_rotation * base.inverse()).normalize();
570}
571
572pub(crate) fn limit_axis_bounds(limits: IkAngleLimit, axis_index: usize) -> (f32, f32) {
573 match axis_index {
574 0 => (limits.min.x, limits.max.x),
575 1 => (limits.min.y, limits.max.y),
576 _ => (limits.min.z, limits.max.z),
577 }
578}
579
580pub(crate) fn axis_vec(axis_index: usize) -> Vec3A {
581 match axis_index {
582 0 => Vec3A::new(1.0, 0.0, 0.0),
583 1 => Vec3A::new(0.0, 1.0, 0.0),
584 _ => Vec3A::new(0.0, 0.0, 1.0),
585 }
586}
587
588pub(crate) fn signed_projected_angle(from: Vec3A, to: Vec3A, axis: Vec3A) -> f32 {
589 let projected_from = from - axis * from.dot(axis);
590 let projected_to = to - axis * to.dot(axis);
591 if projected_from.length_squared() <= f32::EPSILON
592 || projected_to.length_squared() <= f32::EPSILON
593 {
594 return 0.0;
595 }
596 let from_n = projected_from.normalize();
597 let to_n = projected_to.normalize();
598 let dot = from_n.dot(to_n).clamp(-1.0, 1.0);
599 let angle = dot.acos();
600 let sign = axis.dot(from_n.cross(to_n)).signum();
601 angle * if sign == 0.0 { 1.0 } else { sign }
602}
603
604pub(crate) struct PlaneLinkStepInput<'a> {
605 pub local_effector: &'a Vec3A,
606 pub local_target: &'a Vec3A,
607 pub link_index: usize,
608 pub base_rotations: &'a [Quat],
609 pub ik_rotations: &'a mut [Quat],
610 pub chain_states: &'a mut [ChainLinkState],
611 pub axis_index: usize,
612 pub limits: IkAngleLimit,
613 pub iteration: usize,
614 pub limit_angle: f32,
615}
616
617pub(crate) fn solve_plane_link_step(input: PlaneLinkStepInput<'_>) {
618 let rotate_axis = match input.axis_index {
619 0 => Vec3A::new(1.0, 0.0, 0.0),
620 1 => Vec3A::new(0.0, 1.0, 0.0),
621 _ => Vec3A::new(0.0, 0.0, 1.0),
622 };
623 let local_eff_n = input.local_effector.normalize();
624 let local_tgt_n = input.local_target.normalize();
625
626 let dot = local_eff_n.dot(local_tgt_n).clamp(-1.0, 1.0);
627 let raw_angle = dot.acos();
628 let capped_angle = if input.limit_angle > 0.0 {
629 raw_angle.min(input.limit_angle)
630 } else {
631 raw_angle
632 };
633
634 let target_vec1 =
635 Quat::from_axis_angle(rotate_axis.into(), capped_angle).mul_vec3a(local_eff_n);
636 let target_vec2 =
637 Quat::from_axis_angle(rotate_axis.into(), -capped_angle).mul_vec3a(local_eff_n);
638 let signed_angle = if target_vec1.dot(local_tgt_n) > target_vec2.dot(local_tgt_n) {
639 capped_angle
640 } else {
641 -capped_angle
642 };
643
644 let state = &mut input.chain_states[input.link_index];
645 let mut next_angle = state.plane_mode_angle + signed_angle;
646 let (lower, upper) = match input.axis_index {
647 0 => (input.limits.min.x, input.limits.max.x),
648 1 => (input.limits.min.y, input.limits.max.y),
649 _ => (input.limits.min.z, input.limits.max.z),
650 };
651 let base = input.base_rotations[input.link_index];
652
653 if input.iteration == 0 && (next_angle < lower || next_angle > upper) {
654 if -next_angle > lower && -next_angle < upper {
655 next_angle = -next_angle;
656 } else {
657 let half = (lower + upper) * 0.5;
658 if (half - next_angle).abs() > (half + next_angle).abs() {
659 next_angle = -next_angle;
660 }
661 }
662 }
663
664 state.plane_mode_angle = next_angle.clamp(lower, upper);
665 let chain_rotation = Quat::from_axis_angle(rotate_axis.into(), state.plane_mode_angle);
666 input.ik_rotations[input.link_index] = (chain_rotation * base.inverse()).normalize();
667}
668
669#[cfg(test)]
670mod tests {
671 use std::sync::Arc;
672
673 use super::*;
674 use crate::{BoneIndex, BoneInit, IkLinkInit, IkSolverInit, ModelArena, RuntimeInstance};
675
676 fn assert_vec3a_near(actual: Vec3A, expected: Vec3A) {
677 let delta = (actual - expected).abs();
678 assert!(
679 delta.x < 1.0e-5 && delta.y < 1.0e-5 && delta.z < 1.0e-5,
680 "actual={actual:?} expected={expected:?} delta={delta:?}"
681 );
682 }
683
684 fn assert_quat_near(actual: Quat, expected: Quat) {
685 let actual = actual.to_array();
686 let expected = expected.to_array();
687 let delta = [
688 (actual[0] - expected[0]).abs(),
689 (actual[1] - expected[1]).abs(),
690 (actual[2] - expected[2]).abs(),
691 (actual[3] - expected[3]).abs(),
692 ];
693 assert!(
694 delta[0] < 1.0e-5 && delta[1] < 1.0e-5 && delta[2] < 1.0e-5 && delta[3] < 1.0e-5,
695 "actual={actual:?} expected={expected:?} delta={delta:?}"
696 );
697 }
698
699 fn one_link_definition(angle_limit: Option<IkAngleLimit>) -> IkChainDefinition {
700 IkChainDefinition {
701 parent_slots: vec![None, Some(0)],
702 rest_positions: vec![Vec3A::ZERO, Vec3A::X],
703 fixed_axes: vec![None, None],
704 target_slot: 1,
705 links: vec![IkChainLinkDefinition {
706 bone_slot: 0,
707 angle_limit,
708 }],
709 iteration_count: 1,
710 limit_angle: 0.0,
711 }
712 }
713
714 #[test]
715 fn mini_chain_world_update_uses_identity_when_parent_world_is_unspecified() {
716 let definition = one_link_definition(None);
717 let mut world = vec![Mat4::IDENTITY; 2];
718 update_mini_chain_world_matrices(
719 &definition,
720 Mat4::IDENTITY,
721 &[Vec3A::ZERO, Vec3A::new(0.0, 2.0, 0.0)],
722 &[
723 Quat::from_rotation_z(std::f32::consts::FRAC_PI_2),
724 Quat::IDENTITY,
725 ],
726 &mut world,
727 );
728
729 assert_vec3a_near(translation(world[1]), Vec3A::new(-2.0, 1.0, 0.0));
730 }
731
732 #[test]
733 fn primitive_matches_full_runtime_for_unconstrained_chain() {
734 let model = Arc::new(
735 ModelArena::new_with_ik(
736 vec![
737 BoneInit::new(None, Vec3A::ZERO),
738 BoneInit::new(Some(BoneIndex(0)), Vec3A::X),
739 BoneInit::new(None, Vec3A::Y),
740 ],
741 vec![IkSolverInit {
742 ik_bone: BoneIndex(2),
743 target_bone: BoneIndex(1),
744 links: vec![IkLinkInit::new(BoneIndex(0))],
745 iteration_count: 1,
746 limit_angle: 0.0,
747 }],
748 )
749 .unwrap(),
750 );
751 let mut runtime = RuntimeInstance::new(model);
752 runtime.evaluate_current_pose();
753
754 let mut solver = IkChainSolver::new(one_link_definition(None));
755 let local_position_offsets = [Vec3A::ZERO; 2];
756 let local_rotations = [Quat::IDENTITY; 2];
757 let output = solver.solve(IkChainPoseInput {
758 parent_world_matrix: None,
759 local_position_offsets: &local_position_offsets,
760 local_rotations: &local_rotations,
761 goal_position: Vec3A::Y,
762 tolerance: 1.0e-2,
763 max_iterations_cap: None,
764 });
765
766 assert_quat_near(
767 output.solved_link_rotations[0],
768 runtime.pose().local_rotation(BoneIndex(0)),
769 );
770 }
771
772 #[test]
773 fn primitive_matches_full_runtime_for_two_link_unconstrained_chain() {
774 let model = Arc::new(
775 ModelArena::new_with_ik(
776 vec![
777 BoneInit::new(None, Vec3A::ZERO),
778 BoneInit::new(Some(BoneIndex(0)), Vec3A::X),
779 BoneInit::new(Some(BoneIndex(1)), Vec3A::X),
780 BoneInit::new(None, Vec3A::new(1.0, 1.0, 0.0)),
781 ],
782 vec![IkSolverInit {
783 ik_bone: BoneIndex(3),
784 target_bone: BoneIndex(2),
785 links: vec![IkLinkInit::new(BoneIndex(1)), IkLinkInit::new(BoneIndex(0))],
786 iteration_count: 4,
787 limit_angle: 0.0,
788 }],
789 )
790 .unwrap(),
791 );
792 let mut runtime = RuntimeInstance::new(model);
793 runtime.evaluate_current_pose();
794
795 let definition = IkChainDefinition {
796 parent_slots: vec![None, Some(0), Some(1)],
797 rest_positions: vec![Vec3A::ZERO, Vec3A::X, Vec3A::X],
798 fixed_axes: vec![None, None, None],
799 target_slot: 2,
800 links: vec![
801 IkChainLinkDefinition {
802 bone_slot: 1,
803 angle_limit: None,
804 },
805 IkChainLinkDefinition {
806 bone_slot: 0,
807 angle_limit: None,
808 },
809 ],
810 iteration_count: 4,
811 limit_angle: 0.0,
812 };
813 let mut solver = IkChainSolver::new(definition);
814 let local_position_offsets = [Vec3A::ZERO; 3];
815 let local_rotations = [Quat::IDENTITY; 3];
816 let output = solver.solve(IkChainPoseInput {
817 parent_world_matrix: None,
818 local_position_offsets: &local_position_offsets,
819 local_rotations: &local_rotations,
820 goal_position: Vec3A::new(1.0, 1.0, 0.0),
821 tolerance: 1.0e-2,
822 max_iterations_cap: None,
823 });
824
825 assert_quat_near(
826 output.solved_link_rotations[0],
827 runtime.pose().local_rotation(BoneIndex(1)),
828 );
829 assert_quat_near(
830 output.solved_link_rotations[1],
831 runtime.pose().local_rotation(BoneIndex(0)),
832 );
833 }
834
835 #[test]
836 fn deform_order_characterization_keeps_known_ik_delta_bounded() {
837 fn solve_one_pass(
838 definition: &IkChainDefinition,
839 goal_position: Vec3A,
840 strict: bool,
841 ) -> Vec<Quat> {
842 let local_position_offsets = vec![Vec3A::ZERO; definition.rest_positions.len()];
843 let mut local_rotations = vec![Quat::IDENTITY; definition.rest_positions.len()];
844 let base_rotations = vec![Quat::IDENTITY; definition.links.len()];
845 let mut ik_rotations = vec![Quat::IDENTITY; definition.links.len()];
846 let mut chain_states = vec![ChainLinkState::default(); definition.links.len()];
847 let mut world_matrices = vec![Mat4::IDENTITY; definition.rest_positions.len()];
848
849 update_mini_chain_world_matrices(
850 definition,
851 Mat4::IDENTITY,
852 &local_position_offsets,
853 &local_rotations,
854 &mut world_matrices,
855 );
856
857 for link_index in 0..definition.links.len() {
858 let link = &definition.links[link_index];
859 let link_slot = link.bone_slot;
860 let link_world = world_matrices[link_slot];
861 let link_pos = translation(link_world);
862 let eff_pos = translation(world_matrices[definition.target_slot]);
863 let link_world_rot = rotation(link_world);
864 let local_effector = link_world_rot.inverse().mul_vec3a(eff_pos - link_pos);
865 let local_target = link_world_rot.inverse().mul_vec3a(goal_position - link_pos);
866
867 solve_link_step(LinkStepInput {
868 local_effector: &local_effector,
869 local_target: &local_target,
870 link_index,
871 base_rotations: &base_rotations,
872 ik_rotations: &mut ik_rotations,
873 chain_states: &mut chain_states,
874 angle_limit: link.angle_limit,
875 iteration: 0,
876 limit_angle: definition.limit_angle,
877 });
878
879 if strict {
880 local_rotations[link_slot] = ik_rotations[link_index].normalize();
881 update_mini_chain_world_matrices(
882 definition,
883 Mat4::IDENTITY,
884 &local_position_offsets,
885 &local_rotations,
886 &mut world_matrices,
887 );
888 }
889 }
890
891 if !strict {
892 for (link_index, link) in definition.links.iter().enumerate() {
893 local_rotations[link.bone_slot] = ik_rotations[link_index].normalize();
894 }
895 update_mini_chain_world_matrices(
896 definition,
897 Mat4::IDENTITY,
898 &local_position_offsets,
899 &local_rotations,
900 &mut world_matrices,
901 );
902 }
903
904 definition
905 .links
906 .iter()
907 .map(|link| local_rotations[link.bone_slot])
908 .collect()
909 }
910
911 let definition = IkChainDefinition {
912 parent_slots: vec![None, Some(0), Some(1)],
913 rest_positions: vec![Vec3A::ZERO, Vec3A::X, Vec3A::X],
914 fixed_axes: vec![None, None, None],
915 target_slot: 2,
916 links: vec![
917 IkChainLinkDefinition {
918 bone_slot: 1,
919 angle_limit: None,
920 },
921 IkChainLinkDefinition {
922 bone_slot: 0,
923 angle_limit: None,
924 },
925 ],
926 iteration_count: 1,
927 limit_angle: 0.0,
928 };
929 let goal_position = Vec3A::new(1.0, 1.0, 0.0);
930
931 let correct_order = solve_one_pass(&definition, goal_position, true);
932 let dependency_order = solve_one_pass(&definition, goal_position, false);
933 let max_angular_delta = correct_order
934 .iter()
935 .zip(&dependency_order)
936 .map(|(correct, dependency)| correct.angle_between(*dependency))
937 .fold(0.0f32, f32::max);
938
939 assert!(
940 max_angular_delta > 0.0,
941 "fixture must characterize a non-zero strict-order vs dependency-order IK delta"
942 );
943 assert!(
944 max_angular_delta <= 0.79,
945 "characterization budget widened unexpectedly: max_angular_delta={max_angular_delta}"
946 );
947 }
948
949 #[test]
950 fn primitive_matches_full_runtime_for_knee_plane_limit() {
951 let limit = IkAngleLimit::new(
952 Vec3A::new(0.0, 0.0, 0.0),
953 Vec3A::new(0.0, 0.0, std::f32::consts::FRAC_PI_4),
954 );
955 let model = Arc::new(
956 ModelArena::new_with_ik(
957 vec![
958 BoneInit::new(None, Vec3A::ZERO),
959 BoneInit::new(Some(BoneIndex(0)), Vec3A::X),
960 BoneInit::new(None, Vec3A::Y),
961 ],
962 vec![IkSolverInit {
963 ik_bone: BoneIndex(2),
964 target_bone: BoneIndex(1),
965 links: vec![IkLinkInit::new(BoneIndex(0)).with_angle_limit(limit)],
966 iteration_count: 1,
967 limit_angle: 0.0,
968 }],
969 )
970 .unwrap(),
971 );
972 let mut runtime = RuntimeInstance::new(model);
973 runtime.evaluate_current_pose();
974
975 let mut solver = IkChainSolver::new(one_link_definition(Some(limit)));
976 let local_position_offsets = [Vec3A::ZERO; 2];
977 let local_rotations = [Quat::IDENTITY; 2];
978 let output = solver.solve(IkChainPoseInput {
979 parent_world_matrix: None,
980 local_position_offsets: &local_position_offsets,
981 local_rotations: &local_rotations,
982 goal_position: Vec3A::Y,
983 tolerance: 1.0e-2,
984 max_iterations_cap: None,
985 });
986
987 assert_quat_near(
988 output.solved_link_rotations[0],
989 runtime.pose().local_rotation(BoneIndex(0)),
990 );
991 }
992
993 #[test]
994 fn primitive_matches_full_runtime_for_limited_axes_chain() {
995 let limit = IkAngleLimit::new(Vec3A::new(0.0, -0.6, -0.6), Vec3A::new(0.0, 0.6, 0.6));
996 let goal = Vec3A::new(0.25, 0.55, 0.80).normalize();
997 let model = Arc::new(
998 ModelArena::new_with_ik(
999 vec![
1000 BoneInit::new(None, Vec3A::ZERO),
1001 BoneInit::new(Some(BoneIndex(0)), Vec3A::X),
1002 BoneInit::new(None, goal),
1003 ],
1004 vec![IkSolverInit {
1005 ik_bone: BoneIndex(2),
1006 target_bone: BoneIndex(1),
1007 links: vec![IkLinkInit::new(BoneIndex(0)).with_angle_limit(limit)],
1008 iteration_count: 1,
1009 limit_angle: 0.0,
1010 }],
1011 )
1012 .unwrap(),
1013 );
1014 let mut runtime = RuntimeInstance::new(model);
1015 runtime.evaluate_current_pose();
1016
1017 let mut solver = IkChainSolver::new(one_link_definition(Some(limit)));
1018 let local_position_offsets = [Vec3A::ZERO; 2];
1019 let local_rotations = [Quat::IDENTITY; 2];
1020 let output = solver.solve(IkChainPoseInput {
1021 parent_world_matrix: None,
1022 local_position_offsets: &local_position_offsets,
1023 local_rotations: &local_rotations,
1024 goal_position: goal,
1025 tolerance: 1.0e-2,
1026 max_iterations_cap: None,
1027 });
1028
1029 assert_quat_near(
1030 output.solved_link_rotations[0],
1031 runtime.pose().local_rotation(BoneIndex(0)),
1032 );
1033 }
1034
1035 #[test]
1036 fn primitive_is_bit_deterministic_in_current_process_profile() {
1037 let definition = IkChainDefinition {
1040 parent_slots: vec![None, Some(0), Some(1)],
1041 rest_positions: vec![Vec3A::ZERO, Vec3A::X, Vec3A::X],
1042 fixed_axes: vec![None, None, None],
1043 target_slot: 2,
1044 links: vec![
1045 IkChainLinkDefinition {
1046 bone_slot: 1,
1047 angle_limit: None,
1048 },
1049 IkChainLinkDefinition {
1050 bone_slot: 0,
1051 angle_limit: None,
1052 },
1053 ],
1054 iteration_count: 4,
1055 limit_angle: 0.0,
1056 };
1057 let local_position_offsets = [Vec3A::ZERO; 3];
1058 let local_rotations = [Quat::IDENTITY; 3];
1059 let input = IkChainPoseInput {
1060 parent_world_matrix: None,
1061 local_position_offsets: &local_position_offsets,
1062 local_rotations: &local_rotations,
1063 goal_position: Vec3A::new(1.0, 1.0, 0.0),
1064 tolerance: 1.0e-2,
1065 max_iterations_cap: None,
1066 };
1067 let mut baseline_solver = IkChainSolver::new(definition.clone());
1068 let expected = baseline_solver.solve(input);
1069
1070 for _ in 0..32 {
1071 let mut solver = IkChainSolver::new(definition.clone());
1072 let actual = solver.solve(input);
1073 assert_eq!(
1074 actual.final_distance.to_bits(),
1075 expected.final_distance.to_bits()
1076 );
1077 assert_eq!(actual.executed_iterations, expected.executed_iterations);
1078 assert_eq!(actual.link_steps, expected.link_steps);
1079 let actual_bits: Vec<_> = actual
1080 .solved_link_rotations
1081 .iter()
1082 .map(|q| q.to_array().map(f32::to_bits))
1083 .collect();
1084 let expected_bits: Vec<_> = expected
1085 .solved_link_rotations
1086 .iter()
1087 .map(|q| q.to_array().map(f32::to_bits))
1088 .collect();
1089 assert_eq!(actual_bits, expected_bits);
1090 }
1091 }
1092}