tawasuyu/llimphi

1//! Constraints de rigging sobre la jerarquía de huesos (estilo Rive): IK de 2
2//! huesos y aim. Corren **después** de que la animación posó el esqueleto,
3//! ajustando rotaciones para satisfacer un objetivo; luego el skinning deforma.
4//!
5//! Convención: los huesos apuntan a lo largo de su eje local **+x** (su hijo se
6//! traslada en +x, el efector está en +x) — la misma que arma `skel`. Todo en
7//! 2D, analítico (sin iteración), sobre `kurbo`.
8
9use kurbo::{Affine, Point, Vec2};
10
11use crate::skel::{BoneId, Pose, Skeleton};
12
13/// Ángulo de rotación de un `Affine` (asume escala ~uniforme y sin shear).
14fn affine_rotation(aff: Affine) -> f64 {
15    let c = aff.as_coeffs(); // [a, b, c, d, e, f]; a=cosθ·sx, b=sinθ·sx
16    c[1].atan2(c[0])
17}
18
19/// Origen (punto de pivote) de un hueso en mundo. La rotación del propio hueso
20/// no mueve su origen, así que sirve aunque vayamos a re-rotarlo.
21fn bone_origin(skel: &Skeleton, bone: BoneId) -> Point {
22    skel.world(bone) * Point::ZERO
23}
24
25fn parent_rotation(skel: &Skeleton, bone: BoneId) -> f64 {
26    match skel.parent(bone) {
27        Some(p) => affine_rotation(skel.world(p)),
28        None => 0.0,
29    }
30}
31
32/// Resuelve **IK de 2 huesos**: ajusta las rotaciones locales de `upper` (padre)
33/// y `lower` (hijo de `upper`) para que el efector — el punto `tip_local` en el
34/// frame de `lower` — alcance `target` (en espacio de mundo del esqueleto).
35/// `flip` elige la solución del codo (arriba/abajo). Si `target` está fuera de
36/// alcance, el brazo se estira hacia él (alcance máximo). Llama `update` al final.
37///
38/// Requiere que los world transforms estén actualizados antes (la animación ya
39/// posó el esqueleto). Las longitudes salen del rig: `L1` = offset `upper→lower`,
40/// `L2` = `|tip_local|`.
41pub fn solve_two_bone_ik(
42    skel: &mut Skeleton,
43    upper: BoneId,
44    lower: BoneId,
45    tip_local: Vec2,
46    target: Point,
47    flip: bool,
48) {
49    let base = bone_origin(skel, upper);
50    let l1 = skel.pose(lower).translation.hypot();
51    let l2 = tip_local.hypot();
52    if l1 < 1e-9 || l2 < 1e-9 {
53        return;
54    }
55    let s = if flip { -1.0 } else { 1.0 };
56
57    let d = target - base;
58    let dist = d.hypot().clamp((l1 - l2).abs() + 1e-6, l1 + l2 - 1e-6);
59    let alpha = d.y.atan2(d.x);
60    let cos_beta = ((dist * dist + l1 * l1 - l2 * l2) / (2.0 * dist * l1)).clamp(-1.0, 1.0);
61    let beta = cos_beta.acos();
62    let cos_elbow = ((l1 * l1 + l2 * l2 - dist * dist) / (2.0 * l1 * l2)).clamp(-1.0, 1.0);
63    let elbow = cos_elbow.acos();
64
65    let theta_upper_world = alpha - s * beta;
66    let theta_lower_local = s * (std::f64::consts::PI - elbow);
67
68    let up = skel.pose(upper);
69    skel.set_pose(
70        upper,
71        Pose {
72            rotation: theta_upper_world - parent_rotation(skel, upper),
73            ..up
74        },
75    );
76    let low = skel.pose(lower);
77    skel.set_pose(
78        lower,
79        Pose {
80            rotation: theta_lower_local,
81            ..low
82        },
83    );
84    skel.update();
85}
86
87/// **Aim**: rota `bone` para que su eje local `forward` apunte desde su origen
88/// hacia `target` (mundo). Llama `update`. Útil para que una cabeza/ojo/torreta
89/// siga a un objetivo.
90pub fn aim_at(skel: &mut Skeleton, bone: BoneId, forward: Vec2, target: Point) {
91    let origin = bone_origin(skel, bone);
92    let d = target - origin;
93    if d.hypot() < 1e-9 || forward.hypot() < 1e-9 {
94        return;
95    }
96    let desired_world = d.y.atan2(d.x) - forward.y.atan2(forward.x);
97    let pose = skel.pose(bone);
98    skel.set_pose(
99        bone,
100        Pose {
101            rotation: desired_world - parent_rotation(skel, bone),
102            ..pose
103        },
104    );
105    skel.update();
106}
107
108#[cfg(test)]
109mod tests {
110    use super::*;
111    use crate::skel::Pose;
112
113    fn two_bone(l1: f64) -> (Skeleton, BoneId, BoneId) {
114        let mut s = Skeleton::new();
115        let a = s.add_bone(None, Pose::identity());
116        let b = s.add_bone(Some(a), Pose::translate(Vec2::new(l1, 0.0)));
117        s.bind();
118        s.update();
119        (s, a, b)
120    }
121
122    fn tip(skel: &Skeleton, lower: BoneId, tip_local: Vec2) -> Point {
123        skel.world(lower) * Point::new(tip_local.x, tip_local.y)
124    }
125
126    #[test]
127    fn ik_alcanza_un_objetivo_dentro_de_rango() {
128        let (mut s, a, b) = two_bone(50.0);
129        let tip_local = Vec2::new(50.0, 0.0); // L2 = 50, alcance 0..100
130        let target = Point::new(60.0, 40.0); // dist ≈ 72 < 100
131        solve_two_bone_ik(&mut s, a, b, tip_local, target, false);
132        let t = tip(&s, b, tip_local);
133        assert!(
134            (t.x - target.x).abs() < 1e-6 && (t.y - target.y).abs() < 1e-6,
135            "tip {t:?} debería alcanzar {target:?}"
136        );
137    }
138
139    #[test]
140    fn ik_fuera_de_rango_se_estira() {
141        let (mut s, a, b) = two_bone(50.0);
142        let tip_local = Vec2::new(50.0, 0.0);
143        let target = Point::new(300.0, 0.0); // muy lejos → estira a ~100 en x
144        solve_two_bone_ik(&mut s, a, b, tip_local, target, false);
145        let t = tip(&s, b, tip_local);
146        assert!((t.x - 100.0).abs() < 1e-3, "x del tip {} ~ 100", t.x);
147        assert!(t.y.abs() < 1e-3, "y del tip {} ~ 0 (en línea al objetivo)", t.y);
148    }
149
150    #[test]
151    fn ik_flip_da_el_otro_codo() {
152        let (mut s, a, b) = two_bone(50.0);
153        let tip_local = Vec2::new(50.0, 0.0);
154        let target = Point::new(60.0, 40.0);
155
156        solve_two_bone_ik(&mut s, a, b, tip_local, target, false);
157        let elbow_down = bone_origin(&s, b);
158        solve_two_bone_ik(&mut s, a, b, tip_local, target, true);
159        let elbow_up = bone_origin(&s, b);
160
161        // Ambas soluciones alcanzan el target, pero el codo cae en lados
162        // opuestos de la línea base→target.
163        assert!(
164            (elbow_down.y - elbow_up.y).abs() > 1.0,
165            "los codos deberían diferir: {elbow_down:?} vs {elbow_up:?}"
166        );
167        let t = tip(&s, b, tip_local);
168        assert!((t.x - target.x).abs() < 1e-6 && (t.y - target.y).abs() < 1e-6);
169    }
170
171    #[test]
172    fn aim_apunta_el_eje_forward_al_objetivo() {
173        let mut s = Skeleton::new();
174        let bone = s.add_bone(None, Pose::identity());
175        s.bind();
176        s.update();
177        aim_at(&mut s, bone, Vec2::new(1.0, 0.0), Point::new(0.0, 10.0));
178        // El eje +x del hueso ahora debe apuntar hacia +y (al objetivo).
179        let dir = s.world(bone) * Point::new(1.0, 0.0);
180        assert!(dir.x.abs() < 1e-9 && dir.y > 0.0, "dir {dir:?} debería ser +y");
181    }
182}