Skip to main content

qualia_core_db/render/skeletal/
ik_motor.rs

1//! Analytical Two-Bone, Constrained FABRIK, Coupled Swing-Twist Reach Cones,
2//! Scapulo-Humeral Rhythm & Biarticular Tendon Coupling (re-exported from `etalon-render`).
3pub use etalon_render::skeletal::ik_motor::*;
4
5#[cfg(test)]
6mod tests {
7    use super::*;
8
9    #[cfg(not(target_arch = "wasm32"))]
10    #[test]
11    fn swing_twist_and_couplings_reexported_zero_heap() {
12        use crate::specialized_libs::computational_geometry::allocation_counter::assert_zero_alloc;
13
14        let cone = SwingTwistConeConstraint::elliptical([1.0, 0.0, 0.0], 0.6, 0.4, -0.5, 0.5);
15        let coupling = ScapuloHumeralCoupling::default();
16        let biarticular = BiarticularLengthConstraint::new(
17            &[
18                BiarticularTerm {
19                    joint_index: 0,
20                    moment_arm_m: 0.03,
21                    axis: [1.0, 0.0, 0.0],
22                },
23                BiarticularTerm {
24                    joint_index: 1,
25                    moment_arm_m: 0.04,
26                    axis: [1.0, 0.0, 0.0],
27                },
28            ],
29            0.04,
30        )
31        .unwrap();
32
33        assert_zero_alloc("qualia_db_swing_twist_reexport", || {
34            let (s, t) = decompose_swing_twist([1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0]);
35            assert!((s[0] - 1.0).abs() < 1e-6 && (t[0] - 1.0).abs() < 1e-6);
36
37            let proj = project_swing_twist_cone(
38                [0.70710677, 0.0, 0.70710677, 0.0],
39                [1.0, 0.0, 0.0],
40                0.6,
41                0.4,
42                -0.5,
43                0.5,
44                1.0,
45            );
46            assert!(cone.is_quaternion_within(proj, 1.0, 1e-4));
47
48            let (gh, st) = coupling.partition_elevation_rad(std::f32::consts::FRAC_PI_2);
49            assert!((gh + st - std::f32::consts::FRAC_PI_2).abs() < 1e-5);
50
51            let mut angles = [0.9_f32, 0.8_f32];
52            let viol = biarticular
53                .project_angles_in_place(&mut angles, None, 1.0)
54                .unwrap();
55            assert!(viol > 0.0);
56            assert!(biarticular.is_satisfied(&angles, 1.0).unwrap());
57        });
58    }
59}