uzor_urx_physics/
lib.rs

1//! # uzor-urx-physics
2//!
3//! Lightweight 3D physics for the URX render family — Wave 15+16:
4//!
5//! - **Collider**: AABB (axis-aligned box) or Sphere.
6//! - **Body**: id, position, velocity, mass, restitution, kind
7//!   (Dynamic / Static / Kinematic).
8//! - **PhysicsWorld**: stores bodies, runs Verlet integration with
9//!   gravity, and emits `Contact` events for every overlapping pair.
10//!
11//! Out of scope (later waves): joints, friction, continuous-collision
12//! resolution, mesh colliders, sleeping bodies. The point of this
13//! crate is to be ENOUGH for URX 3D demos (falling boxes, ball-on-
14//! plane, particle-vs-box) without pulling in `rapier3d`'s 100k LOC.
15//!
16//! No dependency on `uzor-urx-3d`: that's the consumer's job — pass
17//! `Body::position` into the matching `Node::with_translation` each
18//! frame.
19
20pub use glam::Vec3;
21
22#[derive(Debug, Clone, Copy)]
23pub enum Collider {
24    /// Axis-aligned box centred at the body's position with the given
25    /// half-extents on each axis.
26    Aabb { half_extents: Vec3 },
27    /// Sphere centred at the body's position.
28    Sphere { radius: f32 },
29}
30
31impl Collider {
32    pub fn aabb(half_extents: Vec3) -> Self { Self::Aabb { half_extents } }
33    pub fn sphere(radius: f32) -> Self { Self::Sphere { radius } }
34}
35
36/// Body kind controls how the integrator treats the body.
37///
38/// - `Static`: never moves. Gravity ignored. Other bodies bounce off.
39/// - `Kinematic`: moves but only by direct script (no gravity / impulses
40///   applied automatically). Treated as INFINITE mass during contact.
41/// - `Dynamic`: gravity + impulses apply; obeys mass.
42#[derive(Debug, Clone, Copy, PartialEq, Eq)]
43pub enum BodyKind { Dynamic, Static, Kinematic }
44
45#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, PartialOrd, Ord)]
46pub struct BodyId(pub u32);
47
48#[derive(Debug, Clone)]
49pub struct Body {
50    pub id: BodyId,
51    pub kind: BodyKind,
52    pub collider: Collider,
53    pub position: Vec3,
54    pub velocity: Vec3,
55    /// Inverse mass (0.0 means infinite — for Static / Kinematic).
56    pub inv_mass: f32,
57    /// Bounce coefficient in [0, 1]. 0 = inelastic, 1 = elastic.
58    pub restitution: f32,
59    /// Linear damping per second (velocity *= exp(-damping * dt)).
60    pub damping: f32,
61}
62
63impl Body {
64    pub fn dynamic(id: BodyId, collider: Collider, position: Vec3, mass: f32) -> Self {
65        Self {
66            id, kind: BodyKind::Dynamic, collider, position,
67            velocity: Vec3::ZERO,
68            inv_mass: if mass > 1e-6 { 1.0 / mass } else { 0.0 },
69            restitution: 0.4,
70            damping: 0.05,
71        }
72    }
73    pub fn r#static(id: BodyId, collider: Collider, position: Vec3) -> Self {
74        Self {
75            id, kind: BodyKind::Static, collider, position,
76            velocity: Vec3::ZERO, inv_mass: 0.0, restitution: 0.6, damping: 0.0,
77        }
78    }
79    pub fn kinematic(id: BodyId, collider: Collider, position: Vec3) -> Self {
80        Self {
81            id, kind: BodyKind::Kinematic, collider, position,
82            velocity: Vec3::ZERO, inv_mass: 0.0, restitution: 0.4, damping: 0.0,
83        }
84    }
85}
86
87/// Wave 17 — joint between two bodies.
88///
89/// All joints are constraint-only — they generate impulses that
90/// preserve a relationship every frame. No rotational state means we
91/// can't simulate a true 1-DOF hinge axis (that needs body rotation),
92/// so HingeAxis is approximated by a `Pin` joint at the hinge point.
93#[derive(Debug, Clone, Copy)]
94pub enum Joint {
95    /// Keep `a` and `b` at exactly `rest_length` apart along the line
96    /// connecting their centres. Rod / chain link.
97    Distance {
98        a: BodyId,
99        b: BodyId,
100        rest_length: f32,
101    },
102    /// Pin two bodies together at their CURRENT centre offset.
103    /// Useful for welding kinematic and dynamic bodies, or building
104    /// rigid compound shapes from primitives. Acts as a ball joint
105    /// (3-axis translation locked, no rotation lock since bodies
106    /// don't carry rotation in this physics world).
107    Pin {
108        a: BodyId,
109        b: BodyId,
110        /// Target world-space offset of b relative to a — captured at
111        /// `Pin::weld` time.
112        offset: Vec3,
113    },
114}
115
116impl Joint {
117    pub fn distance(a: BodyId, b: BodyId, rest_length: f32) -> Self {
118        Self::Distance { a, b, rest_length: rest_length.max(0.0) }
119    }
120    /// Capture the current world-space offset between `a` and `b` and
121    /// freeze it as the pin target. (Bodies must already be in their
122    /// pinned configuration when you call this.)
123    pub fn weld(world: &PhysicsWorld, a: BodyId, b: BodyId) -> Option<Self> {
124        let pa = world.body(a)?.position;
125        let pb = world.body(b)?.position;
126        Some(Self::Pin { a, b, offset: pb - pa })
127    }
128}
129
130/// Overlap event — `a` < `b` always (lower-id first) to avoid dupes.
131#[derive(Debug, Clone, Copy)]
132pub struct Contact {
133    pub a: BodyId,
134    pub b: BodyId,
135    /// World-space contact normal pointing from `a` to `b`.
136    pub normal: Vec3,
137    /// Penetration depth — positive means overlapping.
138    pub depth: f32,
139}
140
141pub struct PhysicsWorld {
142    pub gravity: Vec3,
143    bodies: Vec<Body>,
144    next_id: u32,
145    joints: Vec<Joint>,
146    /// Sequential-impulse solver iterations per step. More = stiffer
147    /// joints under load, more CPU.
148    pub joint_iterations: u32,
149}
150
151impl Default for PhysicsWorld {
152    fn default() -> Self {
153        Self::new()
154    }
155}
156
157impl PhysicsWorld {
158    pub fn new() -> Self {
159        Self {
160            gravity: Vec3::new(0.0, -9.81, 0.0),
161            bodies: Vec::new(),
162            next_id: 0,
163            joints: Vec::new(),
164            joint_iterations: 8,
165        }
166    }
167
168    pub fn add_joint(&mut self, j: Joint) { self.joints.push(j); }
169    pub fn joints(&self) -> &[Joint] { &self.joints }
170    pub fn clear_joints(&mut self) { self.joints.clear(); }
171
172    pub fn fresh_id(&mut self) -> BodyId {
173        let id = BodyId(self.next_id);
174        self.next_id += 1;
175        id
176    }
177
178    pub fn add(&mut self, body: Body) -> BodyId {
179        let id = body.id;
180        self.bodies.push(body);
181        id
182    }
183
184    /// Convenience: allocate id + insert a dynamic body in one call.
185    pub fn spawn_dynamic(&mut self, collider: Collider, position: Vec3, mass: f32) -> BodyId {
186        let id = self.fresh_id();
187        self.add(Body::dynamic(id, collider, position, mass));
188        id
189    }
190
191    pub fn spawn_static(&mut self, collider: Collider, position: Vec3) -> BodyId {
192        let id = self.fresh_id();
193        self.add(Body::r#static(id, collider, position));
194        id
195    }
196
197    pub fn bodies(&self) -> &[Body] { &self.bodies }
198    pub fn bodies_mut(&mut self) -> &mut [Body] { &mut self.bodies }
199
200    pub fn body(&self, id: BodyId) -> Option<&Body> {
201        self.bodies.iter().find(|b| b.id == id)
202    }
203    pub fn body_mut(&mut self, id: BodyId) -> Option<&mut Body> {
204        self.bodies.iter_mut().find(|b| b.id == id)
205    }
206
207    /// Step the simulation by `dt` seconds.
208    ///
209    /// 1. Apply gravity to dynamic bodies.
210    /// 2. Integrate position (semi-implicit Euler).
211    /// 3. Find all overlapping pairs.
212    /// 4. Resolve each contact (position correction + impulse).
213    /// 5. Return the (post-resolution) contact list.
214    pub fn step(&mut self, dt: f32) -> Vec<Contact> {
215        for b in &mut self.bodies {
216            if b.kind == BodyKind::Dynamic {
217                b.velocity += self.gravity * dt;
218                // Apply linear damping.
219                let damp = (-b.damping * dt).exp();
220                b.velocity *= damp;
221            }
222            if b.kind != BodyKind::Static {
223                let v = b.velocity;
224                b.position += v * dt;
225            }
226        }
227
228        let contacts = self.find_contacts();
229        for c in &contacts {
230            self.resolve_contact(*c);
231        }
232
233        // Wave 17 — Sequential Impulse joint solver. Each iteration
234        // walks every joint and projects its constraint, accumulating
235        // toward stable rest. 8 iterations default is a good balance
236        // for chains up to ~6 links.
237        for _ in 0..self.joint_iterations {
238            for j in 0..self.joints.len() {
239                let joint = self.joints[j];
240                self.resolve_joint(joint);
241            }
242        }
243
244        contacts
245    }
246
247    fn resolve_joint(&mut self, j: Joint) {
248        match j {
249            Joint::Distance { a, b, rest_length } => {
250                let ia = self.bodies.iter().position(|x| x.id == a);
251                let ib = self.bodies.iter().position(|x| x.id == b);
252                let (ia, ib) = match (ia, ib) { (Some(a), Some(b)) => (a, b), _ => return };
253                let pa = self.bodies[ia].position;
254                let pb = self.bodies[ib].position;
255                let d = pb - pa;
256                let dist = d.length();
257                if dist < 1e-6 { return; }
258                let n = d / dist;
259                let c = dist - rest_length;
260                let inv_a = self.bodies[ia].inv_mass;
261                let inv_b = self.bodies[ib].inv_mass;
262                let inv_sum = inv_a + inv_b;
263                if inv_sum < 1e-6 { return; }
264
265                // Position correction.
266                let push = n * (c / inv_sum);
267                if self.bodies[ia].kind != BodyKind::Static {
268                    let p = push * inv_a;
269                    self.bodies[ia].position += p;
270                }
271                if self.bodies[ib].kind != BodyKind::Static {
272                    let p = push * inv_b;
273                    self.bodies[ib].position -= p;
274                }
275
276                // Velocity correction along the constraint axis.
277                let rv = self.bodies[ib].velocity - self.bodies[ia].velocity;
278                let v_along = rv.dot(n);
279                let lambda = -v_along / inv_sum;
280                let impulse = n * lambda;
281                if self.bodies[ia].kind == BodyKind::Dynamic {
282                    let dv = impulse * inv_a;
283                    self.bodies[ia].velocity -= dv;
284                }
285                if self.bodies[ib].kind == BodyKind::Dynamic {
286                    let dv = impulse * inv_b;
287                    self.bodies[ib].velocity += dv;
288                }
289            }
290            Joint::Pin { a, b, offset } => {
291                let ia = self.bodies.iter().position(|x| x.id == a);
292                let ib = self.bodies.iter().position(|x| x.id == b);
293                let (ia, ib) = match (ia, ib) { (Some(a), Some(b)) => (a, b), _ => return };
294                let pa = self.bodies[ia].position;
295                let pb = self.bodies[ib].position;
296                let target = pa + offset;
297                let err = pb - target;
298                let err_len = err.length();
299                if err_len < 1e-6 { return; }
300                let n = err / err_len;
301                let inv_a = self.bodies[ia].inv_mass;
302                let inv_b = self.bodies[ib].inv_mass;
303                let inv_sum = inv_a + inv_b;
304                if inv_sum < 1e-6 { return; }
305
306                // Position correction — move bodies until b - a == offset.
307                let push = n * (err_len / inv_sum);
308                if self.bodies[ia].kind != BodyKind::Static {
309                    let p = push * inv_a;
310                    self.bodies[ia].position += p;
311                }
312                if self.bodies[ib].kind != BodyKind::Static {
313                    let p = push * inv_b;
314                    self.bodies[ib].position -= p;
315                }
316
317                // Velocity match — bodies should drift together along
318                // any axis (full 3-DOF lock for a pin/ball joint).
319                let rv = self.bodies[ib].velocity - self.bodies[ia].velocity;
320                let rv_len = rv.length();
321                if rv_len < 1e-6 { return; }
322                let dir = rv / rv_len;
323                let lambda = -rv_len / inv_sum;
324                let impulse = dir * lambda;
325                if self.bodies[ia].kind == BodyKind::Dynamic {
326                    let dv = impulse * inv_a;
327                    self.bodies[ia].velocity -= dv;
328                }
329                if self.bodies[ib].kind == BodyKind::Dynamic {
330                    let dv = impulse * inv_b;
331                    self.bodies[ib].velocity += dv;
332                }
333            }
334        }
335    }
336
337    /// Run only the collision-detection pass without integrating.
338    /// Useful when the consumer drives positions externally and only
339    /// wants overlap events.
340    pub fn find_contacts(&self) -> Vec<Contact> {
341        let mut out = Vec::new();
342        let n = self.bodies.len();
343        for i in 0..n {
344            for j in (i + 1)..n {
345                if let Some(c) = contact_between(&self.bodies[i], &self.bodies[j]) {
346                    out.push(c);
347                }
348            }
349        }
350        out
351    }
352
353    fn resolve_contact(&mut self, c: Contact) {
354        // Find indices.
355        let ia = self.bodies.iter().position(|b| b.id == c.a);
356        let ib = self.bodies.iter().position(|b| b.id == c.b);
357        let (ia, ib) = match (ia, ib) { (Some(a), Some(b)) => (a, b), _ => return };
358
359        let inv_a = self.bodies[ia].inv_mass;
360        let inv_b = self.bodies[ib].inv_mass;
361        let inv_sum = inv_a + inv_b;
362        if inv_sum < 1e-6 { return; } // both infinite-mass, no impulse
363
364        // Position correction (push apart by `depth` along the normal,
365        // weighted by inverse mass).
366        let correction = c.normal * (c.depth / inv_sum);
367        if self.bodies[ia].kind != BodyKind::Static {
368            let push = correction * inv_a;
369            self.bodies[ia].position -= push;
370        }
371        if self.bodies[ib].kind != BodyKind::Static {
372            let push = correction * inv_b;
373            self.bodies[ib].position += push;
374        }
375
376        // Velocity impulse.
377        let rv = self.bodies[ib].velocity - self.bodies[ia].velocity;
378        let rel_vel_normal = rv.dot(c.normal);
379        if rel_vel_normal > 0.0 { return; } // separating
380
381        let e = self.bodies[ia].restitution.min(self.bodies[ib].restitution);
382        let j = -(1.0 + e) * rel_vel_normal / inv_sum;
383        let impulse = c.normal * j;
384        if self.bodies[ia].kind == BodyKind::Dynamic {
385            let dv = impulse * inv_a;
386            self.bodies[ia].velocity -= dv;
387        }
388        if self.bodies[ib].kind == BodyKind::Dynamic {
389            let dv = impulse * inv_b;
390            self.bodies[ib].velocity += dv;
391        }
392    }
393}
394
395fn contact_between(a: &Body, b: &Body) -> Option<Contact> {
396    let (id_a, id_b) = (a.id, b.id);
397    match (a.collider, b.collider) {
398        (Collider::Aabb { half_extents: ha }, Collider::Aabb { half_extents: hb }) => {
399            aabb_aabb(a.position, ha, b.position, hb).map(|(n, d)| Contact { a: id_a, b: id_b, normal: n, depth: d })
400        }
401        (Collider::Sphere { radius: ra }, Collider::Sphere { radius: rb }) => {
402            sphere_sphere(a.position, ra, b.position, rb).map(|(n, d)| Contact { a: id_a, b: id_b, normal: n, depth: d })
403        }
404        (Collider::Aabb { half_extents }, Collider::Sphere { radius }) => {
405            aabb_sphere(a.position, half_extents, b.position, radius).map(|(n, d)| Contact { a: id_a, b: id_b, normal: n, depth: d })
406        }
407        (Collider::Sphere { radius }, Collider::Aabb { half_extents }) => {
408            aabb_sphere(b.position, half_extents, a.position, radius).map(|(n, d)| Contact { a: id_a, b: id_b, normal: -n, depth: d })
409        }
410    }
411}
412
413fn aabb_aabb(pa: Vec3, ha: Vec3, pb: Vec3, hb: Vec3) -> Option<(Vec3, f32)> {
414    let d = pb - pa;
415    let overlap = ha + hb - d.abs();
416    if overlap.x <= 0.0 || overlap.y <= 0.0 || overlap.z <= 0.0 { return None; }
417    // Resolve along axis of LEAST overlap.
418    let (axis, depth) = if overlap.x < overlap.y && overlap.x < overlap.z {
419        (Vec3::new(d.x.signum(), 0.0, 0.0), overlap.x)
420    } else if overlap.y < overlap.z {
421        (Vec3::new(0.0, d.y.signum(), 0.0), overlap.y)
422    } else {
423        (Vec3::new(0.0, 0.0, d.z.signum()), overlap.z)
424    };
425    Some((axis, depth))
426}
427
428fn sphere_sphere(pa: Vec3, ra: f32, pb: Vec3, rb: f32) -> Option<(Vec3, f32)> {
429    let d = pb - pa;
430    let dist = d.length();
431    let r = ra + rb;
432    if dist >= r { return None; }
433    let n = if dist > 1e-6 { d / dist } else { Vec3::Y };
434    Some((n, r - dist))
435}
436
437fn aabb_sphere(pa: Vec3, ha: Vec3, pb: Vec3, rb: f32) -> Option<(Vec3, f32)> {
438    // Find closest point on the AABB to the sphere centre.
439    let min = pa - ha;
440    let max = pa + ha;
441    let closest = pb.clamp(min, max);
442    let d = pb - closest;
443    let dist = d.length();
444    if dist >= rb { return None; }
445    let n = if dist > 1e-6 { d / dist } else {
446        // Sphere centre inside AABB — push along the axis where it's
447        // closest to a face.
448        let to_min = pb - min;
449        let to_max = max - pb;
450        let axes = [
451            (to_min.x, Vec3::NEG_X), (to_max.x, Vec3::X),
452            (to_min.y, Vec3::NEG_Y), (to_max.y, Vec3::Y),
453            (to_min.z, Vec3::NEG_Z), (to_max.z, Vec3::Z),
454        ];
455        axes.iter().fold((f32::INFINITY, Vec3::Y), |acc, &(v, a)| {
456            if v < acc.0 { (v, a) } else { acc }
457        }).1
458    };
459    Some((n, rb - dist))
460}
uzor_urx_physics/lib.rs

uzor_urx_physics/
lib.rs
