tawasuyu/llimphi

1//! Física de partículas (Verlet) + constraints de distancia, para mover
2//! esqueletos con **leyes físicas**: cuerdas, péndulos, ragdolls que caen y se
3//! balancean bajo gravedad, con piso y paredes.
4//!
5//! El patrón es: una cadena de [`Particle`]s unidas por [`Link`]s (distancias
6//! rígidas = longitudes de hueso) se integra con gravedad; luego
7//! [`pose_chain_from_points`] convierte las posiciones de las partículas en poses
8//! de una cadena de huesos, y el skinning deforma la malla. Así la física maneja
9//! el esqueleto, no una animación keyframeada.
10//!
11//! Verlet (en vez de Euler con velocidades explícitas) hace los constraints de
12//! distancia triviales y estables: se resuelven por proyección iterativa.
13
14use kurbo::{Point, Vec2};
15
16use crate::skel::{BoneId, Skeleton};
17
18/// Una partícula puntual. `inv_mass = 0` la fija (pin); `damping` global la frena.
19#[derive(Debug, Clone, Copy)]
20pub struct Particle {
21    pub pos: Point,
22    pub prev: Point,
23    pub inv_mass: f64,
24}
25
26impl Particle {
27    pub fn new(pos: Point, pinned: bool) -> Self {
28        Self {
29            pos,
30            prev: pos,
31            inv_mass: if pinned { 0.0 } else { 1.0 },
32        }
33    }
34    pub fn pinned(&self) -> bool {
35        self.inv_mass == 0.0
36    }
37}
38
39/// Restricción de distancia entre dos partículas (un "hueso" rígido).
40#[derive(Debug, Clone, Copy)]
41pub struct Link {
42    pub a: usize,
43    pub b: usize,
44    pub rest: f64,
45    /// 0..1: 1 = rígido, <1 = elástico.
46    pub stiffness: f64,
47}
48
49/// Mundo físico 2D: partículas + links + gravedad + piso/paredes.
50#[derive(Debug, Clone)]
51pub struct Physics {
52    pub particles: Vec<Particle>,
53    pub links: Vec<Link>,
54    pub gravity: Vec2,
55    /// Factor de retención de velocidad por step (0..1, ~0.99).
56    pub damping: f64,
57    /// Si está, las partículas no bajan de esta `y` (piso).
58    pub floor_y: Option<f64>,
59    /// Paredes `(x0, x1)` que contienen en x.
60    pub walls_x: Option<(f64, f64)>,
61}
62
63impl Default for Physics {
64    fn default() -> Self {
65        Self {
66            particles: Vec::new(),
67            links: Vec::new(),
68            gravity: Vec2::new(0.0, 980.0), // px/s² hacia abajo (y crece para abajo)
69            damping: 0.99,
70            floor_y: None,
71            walls_x: None,
72        }
73    }
74}
75
76impl Physics {
77    pub fn new() -> Self {
78        Self::default()
79    }
80
81    /// Agrega una partícula y devuelve su índice.
82    pub fn particle(&mut self, pos: Point, pinned: bool) -> usize {
83        let i = self.particles.len();
84        self.particles.push(Particle::new(pos, pinned));
85        i
86    }
87
88    /// Une dos partículas con `rest` = su distancia actual y rigidez total.
89    pub fn link(&mut self, a: usize, b: usize) {
90        let rest = (self.particles[b].pos - self.particles[a].pos).hypot();
91        self.links.push(Link { a, b, rest, stiffness: 1.0 });
92    }
93
94    /// Une dos partículas con una distancia de reposo y rigidez dadas.
95    pub fn link_with(&mut self, a: usize, b: usize, rest: f64, stiffness: f64) {
96        self.links.push(Link { a, b, rest, stiffness });
97    }
98
99    /// Empuja las partículas lejos de `center` dentro de `radius` (campo de
100    /// repulsión — p. ej. el cursor barriendo las cuerdas).
101    pub fn repel(&mut self, center: Point, radius: f64, strength: f64) {
102        for p in &mut self.particles {
103            if p.pinned() {
104                continue;
105            }
106            let d = p.pos - center;
107            let dist = d.hypot();
108            if dist < radius && dist > 1e-6 {
109                let push = (1.0 - dist / radius) * strength;
110                p.pos += d * (push / dist);
111            }
112        }
113    }
114
115    /// Un paso de simulación: integra (gravedad + inercia) y resuelve los
116    /// constraints `iterations` veces (más iteraciones = más rígido/estable).
117    pub fn step(&mut self, dt: f64, iterations: usize) {
118        let dt2 = dt * dt;
119        for p in &mut self.particles {
120            if p.pinned() {
121                continue;
122            }
123            let vel = (p.pos - p.prev) * self.damping;
124            p.prev = p.pos;
125            p.pos = p.pos + vel + self.gravity * dt2;
126        }
127        for _ in 0..iterations.max(1) {
128            self.solve_links();
129            self.solve_bounds();
130        }
131    }
132
133    fn solve_links(&mut self) {
134        for k in 0..self.links.len() {
135            let Link { a, b, rest, stiffness } = self.links[k];
136            let pa = self.particles[a].pos;
137            let pb = self.particles[b].pos;
138            let wa = self.particles[a].inv_mass;
139            let wb = self.particles[b].inv_mass;
140            let wsum = wa + wb;
141            if wsum < 1e-12 {
142                continue;
143            }
144            let delta = pb - pa;
145            let d = delta.hypot();
146            if d < 1e-9 {
147                continue;
148            }
149            let diff = (d - rest) / d * stiffness;
150            let corr = delta * diff;
151            self.particles[a].pos = pa + corr * (wa / wsum);
152            self.particles[b].pos = pb - corr * (wb / wsum);
153        }
154    }
155
156    fn solve_bounds(&mut self) {
157        for p in &mut self.particles {
158            if p.pinned() {
159                continue;
160            }
161            if let Some(fy) = self.floor_y {
162                if p.pos.y > fy {
163                    p.pos.y = fy;
164                    // Fricción: amortigua el deslizamiento horizontal en el piso.
165                    p.prev.x = p.pos.x + (p.prev.x - p.pos.x) * 0.5;
166                }
167            }
168            if let Some((x0, x1)) = self.walls_x {
169                p.pos.x = p.pos.x.clamp(x0, x1);
170            }
171        }
172    }
173
174    /// Posiciones actuales de las partículas (para construir poses / pintar).
175    pub fn positions(&self) -> Vec<Point> {
176        self.particles.iter().map(|p| p.pos).collect()
177    }
178}
179
180/// Posa una cadena de huesos para que siga una cadena de puntos (las partículas
181/// físicas). `points` tiene `bones.len() + 1` entradas: el hueso `i` va de
182/// `points[i]` a `points[i+1]`. Setea la translación de la raíz a `points[0]` y
183/// las rotaciones a los ángulos de cada segmento (relativos en la jerarquía).
184/// Llama `update`. Es el puente física → esqueleto.
185pub fn pose_chain_from_points(skel: &mut Skeleton, bones: &[BoneId], points: &[Point]) {
186    if bones.is_empty() || points.len() < bones.len() + 1 {
187        return;
188    }
189    let seg_angle = |i: usize| {
190        let d = points[i + 1] - points[i];
191        if d.hypot() < 1e-9 {
192            0.0
193        } else {
194            d.y.atan2(d.x)
195        }
196    };
197
198    // Raíz: su origen va a points[0]; rotación = ángulo del primer segmento.
199    let root = bones[0];
200    let mut rp = skel.pose(root);
201    rp.translation = points[0].to_vec2();
202    rp.rotation = seg_angle(0);
203    skel.set_pose(root, rp);
204
205    // Eslabones: rotación local = ángulo del segmento − ángulo del anterior
206    // (la rotación world se acumula por la jerarquía).
207    for i in 1..bones.len() {
208        let mut p = skel.pose(bones[i]);
209        p.rotation = seg_angle(i) - seg_angle(i - 1);
210        skel.set_pose(bones[i], p);
211    }
212    skel.update();
213}
214
215#[cfg(test)]
216mod tests {
217    use super::*;
218    use crate::skel::{Pose, Skeleton};
219
220    #[test]
221    fn la_gravedad_hace_caer_una_particula_libre() {
222        let mut w = Physics::new();
223        let i = w.particle(Point::new(0.0, 0.0), false);
224        w.step(0.1, 1);
225        assert!(w.particles[i].pos.y > 0.0, "debería caer (y crece)");
226    }
227
228    #[test]
229    fn una_particula_fija_no_se_mueve() {
230        let mut w = Physics::new();
231        let i = w.particle(Point::new(5.0, 5.0), true);
232        for _ in 0..10 {
233            w.step(0.1, 2);
234        }
235        assert_eq!(w.particles[i].pos, Point::new(5.0, 5.0));
236    }
237
238    #[test]
239    fn un_pendulo_cuelga_a_la_distancia_de_reposo() {
240        let mut w = Physics::new();
241        let anchor = w.particle(Point::new(0.0, 0.0), true);
242        let bob = w.particle(Point::new(0.0, 50.0), false);
243        w.link(anchor, bob); // rest = 50
244        for _ in 0..400 {
245            w.step(1.0 / 120.0, 8);
246        }
247        // En reposo cuelga recto hacia abajo a ~50 del ancla.
248        let p = w.particles[bob].pos;
249        assert!((p.x).abs() < 1.0, "x ~ 0, fue {}", p.x);
250        assert!((p.y - 50.0).abs() < 1.0, "y ~ 50, fue {}", p.y);
251    }
252
253    #[test]
254    fn el_piso_detiene_la_caida() {
255        let mut w = Physics::new();
256        w.floor_y = Some(100.0);
257        let i = w.particle(Point::new(0.0, 0.0), false);
258        for _ in 0..200 {
259            w.step(1.0 / 120.0, 2);
260        }
261        assert!(w.particles[i].pos.y <= 100.0 + 1e-6, "no pasa el piso");
262        assert!(w.particles[i].pos.y > 90.0, "llegó cerca del piso");
263    }
264
265    #[test]
266    fn pose_chain_sigue_los_puntos() {
267        // Cadena de 2 huesos; puntos en L vertical-luego-horizontal.
268        let mut s = Skeleton::new();
269        let a = s.add_bone(None, Pose::translate(Vec2::new(0.0, 0.0)));
270        let b = s.add_bone(Some(a), Pose::translate(Vec2::new(50.0, 0.0)));
271        s.bind();
272        let points = [
273            Point::new(0.0, 0.0),
274            Point::new(0.0, 50.0),  // primer segmento apunta hacia abajo (+y)
275            Point::new(50.0, 50.0), // segundo apunta a la derecha (+x)
276        ];
277        pose_chain_from_points(&mut s, &[a, b], &points);
278        // El origen del hueso b debe estar en points[1]; su punta (local 50,0) en points[2].
279        let b_origin = s.world(b) * Point::ZERO;
280        assert!((b_origin.x - 0.0).abs() < 1e-6 && (b_origin.y - 50.0).abs() < 1e-6, "{b_origin:?}");
281        let tip = s.world(b) * Point::new(50.0, 0.0);
282        assert!((tip.x - 50.0).abs() < 1e-6 && (tip.y - 50.0).abs() < 1e-6, "tip {tip:?}");
283    }
284}