Skip to main content

tpt_sci_physics_rigid/
lib.rs

1//! # tpt-sci-physics-rigid
2//!
3//! A small, dependency-light **rigid-body (sphere) physics world** with analytic
4//! collision resolution, implemented entirely from scratch (no wrapped physics
5//! engine such as `rapier`). Bodies are spheres of arbitrary dimension (2-D or
6//! 3-D) described by [`Body`], simulated inside a [`World`] that supports
7//! constant gravity, axis-aligned bounding walls, and pairwise elastic
8//! collisions with a configurable restitution coefficient and an optional
9//! global Coulomb friction coefficient ([`World::set_friction`]).
10//!
11//! The integrator is a semi-implicit (symplectic) Euler scheme; collisions are
12//! resolved with the standard two-sphere normal impulse, a tangential impulse
13//! bounded by Coulomb's law (`|friction_impulse| <= mu * |normal_impulse|`),
14//! plus a positional (Baumgarte-style) correction to keep overlapping bodies
15//! from sinking into one another. Candidate colliding pairs are found with a
16//! sweep-and-prune broad-phase rather than an all-pairs `O(n^2)` scan.
17//!
18//! Each [`Body`] is a genuine rigid body, not just a point mass: it carries an
19//! orientation quaternion and an angular velocity, so torques change its spin
20//! and its orientation integrates forward under [`Body::spin`]. For a sphere the
21//! moment of inertia is isotropic (default `(2/5)·m·r²`), captured by a single
22//! scalar [`Body::inertia`].
23//!
24//! # Examples
25//!
26//! ```
27//! use tpt_sci_physics_rigid::{Body, World, PhysicsError};
28//! use tpt_math_linalg::tpt_math_linalg_dense::DVector;
29//!
30//! let mut world = World::new();
31//! let a = Body::new(
32//!     0,
33//!     DVector::from_row_slice(&[0.0_f64, 0.0]),
34//!     DVector::from_row_slice(&[1.0_f64, 0.0]),
35//!     1.0,
36//!     0.5,
37//! )?;
38//! world.add_body(a).unwrap();
39//! let b = Body::new(
40//!     1,
41//!     DVector::from_row_slice(&[1.0_f64, 0.0]),
42//!     DVector::from_row_slice(&[0.0_f64, 0.0]),
43//!     1.0,
44//!     0.5,
45//! )?;
46//! world.add_body(b).unwrap();
47//!
48//! let p0 = world.body(0).unwrap().position.clone();
49//! world.step(0.5);
50//! let p1 = world.body(0).unwrap().position.clone();
51//! assert!((p1[0] - p0[0]).abs() > 1e-9);
52//! # Ok::<(), PhysicsError>(())
53//! ```
54#![forbid(unsafe_code)]
55
56use tpt_math_linalg::tpt_math_linalg_dense::{DMatrix, DVector};
57
58mod error;
59
60pub use error::PhysicsError;
61
62mod body;
63mod collision;
64mod quaternion;
65mod world;
66
67pub use body::Body;
68#[cfg(test)]
69use collision::{aabb_overlap, normalize};
70pub(crate) use collision::{resolve_pair, sap_candidate_pairs};
71pub use quaternion::{quat_mul, quat_normalize, quat_to_matrix};
72pub use world::World;
73
74#[cfg(test)]
75mod tests {
76    use super::*;
77    use approx::assert_abs_diff_eq;
78
79    fn v(x: &[f64]) -> DVector<f64> {
80        DVector::from_row_slice(x)
81    }
82
83    #[test]
84    fn body_new_rejects_invalid() {
85        assert!(Body::new(0, v(&[0.0]), v(&[1.0]), 0.0, 0.5).is_err());
86        assert!(Body::new(0, v(&[0.0]), v(&[1.0]), 1.0, 0.0).is_err());
87        assert!(Body::new(0, v(&[0.0]), v(&[1.0, 0.0]), 1.0, 0.5).is_err());
88        assert!(Body::new(0, v(&[f64::NAN]), v(&[0.0]), 1.0, 0.5).is_err());
89        assert!(Body::new(0, v(&[0.0]), v(&[1.0]), 1.0, 0.5).is_ok());
90    }
91
92    #[test]
93    fn duplicate_id_rejected() {
94        let mut world = World::new();
95        world
96            .add_body(Body::new(0, v(&[0.0]), v(&[0.0]), 1.0, 0.5).unwrap())
97            .unwrap();
98        let err = world
99            .add_body(Body::new(0, v(&[1.0]), v(&[0.0]), 1.0, 0.5).unwrap())
100            .unwrap_err();
101        assert!(matches!(err, PhysicsError::DuplicateId(0)));
102    }
103
104    #[test]
105    fn elastic_head_on_swaps_velocities() {
106        let mut world = World::new();
107        // Centers exactly r_i + r_j apart => touching; they overlap after dt.
108        world
109            .add_body(Body::new(0, v(&[0.0, 0.0]), v(&[1.0, 0.0]), 1.0, 0.5).unwrap())
110            .unwrap();
111        world
112            .add_body(Body::new(1, v(&[1.0, 0.0]), v(&[0.0, 0.0]), 1.0, 0.5).unwrap())
113            .unwrap();
114
115        let p_before: f64 = world.bodies().iter().map(|b| b.mass * b.velocity[0]).sum();
116        let ke_before: f64 = world
117            .bodies()
118            .iter()
119            .map(|b| 0.5 * b.mass * b.velocity.dot(&b.velocity))
120            .sum();
121
122        world.step(0.5);
123
124        let a = world.body(0).unwrap();
125        let b = world.body(1).unwrap();
126        assert_abs_diff_eq!(a.velocity[0], 0.0, epsilon = 1e-6);
127        assert_abs_diff_eq!(b.velocity[0], 1.0, epsilon = 1e-6);
128        assert_abs_diff_eq!(a.velocity[1], 0.0, epsilon = 1e-6);
129        assert_abs_diff_eq!(b.velocity[1], 0.0, epsilon = 1e-6);
130
131        let p_after: f64 = world.bodies().iter().map(|b| b.mass * b.velocity[0]).sum();
132        let ke_after: f64 = world
133            .bodies()
134            .iter()
135            .map(|b| 0.5 * b.mass * b.velocity.dot(&b.velocity))
136            .sum();
137        assert_abs_diff_eq!(p_after, p_before, epsilon = 1e-6);
138        assert_abs_diff_eq!(ke_after, ke_before, epsilon = 1e-6);
139        assert_abs_diff_eq!(p_before, 1.0, epsilon = 1e-6);
140        assert_abs_diff_eq!(ke_before, 0.5, epsilon = 1e-6);
141    }
142
143    #[test]
144    fn gravity_free_fall() {
145        let mut world = World::with_gravity(v(&[0.0, -9.8]));
146        world
147            .add_body(Body::new(0, v(&[0.0, 10.0]), v(&[0.0, 0.0]), 1.0, 0.5).unwrap())
148            .unwrap();
149        world.step(1.0);
150        let b = world.body(0).unwrap();
151        // Δy = ½·g·t² = -4.9
152        assert_abs_diff_eq!(b.position[1], 10.0 - 4.9, epsilon = 1e-6);
153        assert_abs_diff_eq!(b.velocity[1], -9.8, epsilon = 1e-6);
154    }
155
156    #[test]
157    fn wall_bounce() {
158        let mut world = World::new();
159        world.set_bounds(v(&[5.0, 5.0]));
160        world
161            .add_body(Body::new(0, v(&[4.8, 0.0]), v(&[1.0, 0.0]), 1.0, 0.5).unwrap())
162            .unwrap();
163        world.step(0.5);
164        let b = world.body(0).unwrap();
165        assert!(b.velocity[0] < 0.0, "velocity should be reflected");
166        // Sphere must be fully inside the box: |p ± r| within half-extents.
167        assert!(b.position[0] - b.radius >= -5.0 - 1e-9);
168        assert!(b.position[0] + b.radius <= 5.0 + 1e-9);
169        assert!(b.position[0].abs() <= 5.0 + 1e-9);
170    }
171
172    #[test]
173    fn momentum_conserved_multi_body_elastic() {
174        let mut world = World::new();
175        world
176            .add_body(Body::new(0, v(&[0.0, 0.0]), v(&[1.0, 0.0]), 1.0, 0.5).unwrap())
177            .unwrap();
178        world
179            .add_body(Body::new(1, v(&[1.0, 0.0]), v(&[0.0, 0.0]), 1.0, 0.5).unwrap())
180            .unwrap();
181
182        let before: DVector<f64> = world.bodies().iter().fold(DVector::zeros(2), |acc, b| {
183            acc + (b.velocity.clone() * b.mass)
184        });
185
186        world.step(0.5);
187
188        let after: DVector<f64> = world.bodies().iter().fold(DVector::zeros(2), |acc, b| {
189            acc + (b.velocity.clone() * b.mass)
190        });
191
192        assert_abs_diff_eq!(after[0], before[0], epsilon = 1e-6);
193        assert_abs_diff_eq!(after[1], before[1], epsilon = 1e-6);
194    }
195
196    #[test]
197    fn apply_torque_changes_spin() {
198        let mut b = Body::new(0, v(&[0.0, 0.0]), v(&[0.0, 0.0]), 2.0, 0.5).unwrap();
199        // Default inertia for a sphere = 0.4·m·r² = 0.4·2·0.25 = 0.2.
200        assert_abs_diff_eq!(b.inertia, 0.2, epsilon = 1e-12);
201        // τ = (0,0,1) for dt = 0.1  =>  ω_z = τ·dt / I = 0.1/0.2 = 0.5.
202        b.apply_torque([0.0, 0.0, 1.0], 0.1);
203        assert_abs_diff_eq!(b.angular_velocity[2], 0.5, epsilon = 1e-12);
204        assert_abs_diff_eq!(b.angular_velocity[0], 0.0, epsilon = 1e-12);
205    }
206
207    #[test]
208    fn spin_integrates_orientation() {
209        let mut b = Body::new(0, v(&[0.0, 0.0]), v(&[0.0, 0.0]), 1.0, 0.5).unwrap();
210        b.set_angular_velocity([0.0, 0.0, 1.0]); // spin about z
211        let dt = 0.01_f64;
212        let theta = 1.0 * dt; // small angle
213        b.spin(dt);
214        let r = b.orientation_matrix();
215        // Rotating the x-axis by `theta` about z should give (cosθ, sinθ, 0).
216        assert_abs_diff_eq!(r[(0, 0)], theta.cos(), epsilon = 1e-3);
217        assert_abs_diff_eq!(r[(1, 0)], theta.sin(), epsilon = 1e-3);
218        // The quaternion must stay (approximately) unit.
219        let q = b.orientation;
220        assert_abs_diff_eq!(
221            (q[0].powi(2) + q[1].powi(2) + q[2].powi(2) + q[3].powi(2)).sqrt(),
222            1.0,
223            epsilon = 1e-9
224        );
225    }
226
227    #[test]
228    fn quat_helpers_roundtrip() {
229        let q = [0.0, 0.0, 0.0, 1.0]; // 180° about z
230        let r = quat_to_matrix(&q);
231        // Maps (1,0,0) -> (-1,0,0) and (0,1,0) -> (0,-1,0).
232        assert_abs_diff_eq!(r[(0, 0)], -1.0, epsilon = 1e-9);
233        assert_abs_diff_eq!(r[(1, 1)], -1.0, epsilon = 1e-9);
234        assert_abs_diff_eq!(r[(2, 2)], 1.0, epsilon = 1e-9);
235    }
236
237    /// Build a world with a sphere dropping under gravity onto the floor
238    /// (a bounding wall) while sliding sideways (nonzero tangential
239    /// velocity), and return the tangential (x-axis) speed after `steps`
240    /// small time steps.
241    fn sliding_tangential_speed(mu: f64, steps: usize) -> f64 {
242        let mut world = World::with_gravity(v(&[0.0, -9.8]));
243        world.set_bounds(v(&[100.0, 5.0]));
244        world.set_restitution(0.0); // inelastic, so the body stays on the floor
245        world.set_friction(mu);
246        world
247            .add_body(Body::new(0, v(&[0.0, 4.6]), v(&[2.0, 0.0]), 1.0, 0.5).unwrap())
248            .unwrap();
249        let dt = 0.01;
250        for _ in 0..steps {
251            world.step(dt);
252        }
253        world.body(0).unwrap().velocity[0].abs()
254    }
255
256    #[test]
257    fn wall_friction_decelerates_tangential_slide() {
258        let speed_no_friction = sliding_tangential_speed(0.0, 200);
259        let speed_with_friction = sliding_tangential_speed(0.8, 200);
260        // Without friction the tangential (x) velocity is unaffected by the
261        // (purely normal) wall bounce; with friction it must be strictly
262        // smaller after the same number of steps sliding on the floor.
263        assert_abs_diff_eq!(speed_no_friction, 2.0, epsilon = 1e-6);
264        assert!(
265            speed_with_friction < speed_no_friction,
266            "friction should reduce tangential speed: {speed_with_friction} vs {speed_no_friction}"
267        );
268    }
269
270    #[test]
271    fn body_body_friction_decelerates_tangential_relative_velocity() {
272        // Two overlapping spheres centered along the x-axis (so the contact
273        // normal is exactly x), with `a` approaching `b` along x while also
274        // sliding relative to `b` along y (tangential to the contact).
275        // Resolve directly via `resolve_pair` (bypassing `World::step`'s
276        // position integration, which would otherwise perturb the contact
277        // normal) so the normal/tangential decomposition is exact.
278        fn run(mu: f64) -> f64 {
279            let mut a = Body::new(0, v(&[0.0, 0.0]), v(&[1.0, 1.0]), 1.0, 0.5).unwrap();
280            let mut b = Body::new(1, v(&[0.9, 0.0]), v(&[0.0, 0.0]), 1.0, 0.5).unwrap();
281            resolve_pair(&mut a, &mut b, 0.0, mu); // inelastic (restitution = 0)
282            let rel = b.velocity - a.velocity;
283            rel[1].abs() // relative tangential (y) speed after collision
284        }
285
286        let rel_tangential_no_friction = run(0.0);
287        let rel_tangential_with_friction = run(0.9);
288        assert_abs_diff_eq!(rel_tangential_no_friction, 1.0, epsilon = 1e-6);
289        assert!(
290            rel_tangential_with_friction < rel_tangential_no_friction,
291            "friction should reduce relative tangential speed: {rel_tangential_with_friction} vs {rel_tangential_no_friction}"
292        );
293    }
294
295    #[test]
296    fn friction_impulse_never_exceeds_coulomb_bound() {
297        // Directly probe `resolve_pair` at a variety of `mu` and check that
298        // the total impulse magnitude applied to `b` decomposes into a normal
299        // part `j_n` and tangential part `j_t` with `j_t <= mu * j_n` (up to
300        // float slop).
301        for &mu in &[0.0_f64, 0.1, 0.5, 1.0, 2.5] {
302            let mut a = Body::new(0, v(&[0.0, 0.0]), v(&[1.0, 0.5]), 1.0, 0.5).unwrap();
303            let mut b = Body::new(1, v(&[0.9, 0.0]), v(&[-0.5, -0.2]), 2.0, 0.5).unwrap();
304            let v_before = b.velocity.clone();
305            resolve_pair(&mut a, &mut b, 0.3, mu);
306            let delta_v = b.velocity.clone() - v_before;
307            let normal = normalize(&(v(&[0.9, 0.0]) - v(&[0.0, 0.0])));
308            let j_n = delta_v.dot(&normal); // impulse/mass along the normal
309            let tangential = delta_v.clone() - (normal.clone() * j_n);
310            let j_t = tangential.norm();
311            assert!(
312                j_t <= mu * j_n.abs() + 1e-9,
313                "mu={mu}: j_t={j_t} exceeds mu*j_n={}",
314                mu * j_n.abs()
315            );
316        }
317    }
318
319    #[test]
320    fn sap_matches_brute_force_pair_set() {
321        // Deterministic pseudo-random body layout (linear congruential
322        // generator, no external `rand` dependency) with several dozen
323        // bodies of varying radii, some clustered (overlapping) and some far
324        // apart, in 3-D.
325        fn lcg_next(state: &mut u64) -> f64 {
326            *state = state.wrapping_mul(6364136223846793005).wrapping_add(1);
327            ((*state >> 33) as f64) / (u32::MAX as f64) // in [0, 1)
328        }
329
330        let mut state = 42_u64;
331        let mut bodies = Vec::new();
332        for id in 0..60 {
333            let x = lcg_next(&mut state) * 20.0 - 10.0;
334            let y = lcg_next(&mut state) * 20.0 - 10.0;
335            let z = lcg_next(&mut state) * 20.0 - 10.0;
336            let r = 0.1 + lcg_next(&mut state) * 0.9;
337            bodies.push(Body::new(id, v(&[x, y, z]), v(&[0.0, 0.0, 0.0]), 1.0, r).unwrap());
338        }
339
340        let sap: std::collections::HashSet<(usize, usize)> =
341            sap_candidate_pairs(&bodies).into_iter().collect();
342
343        let mut brute = std::collections::HashSet::new();
344        for i in 0..bodies.len() {
345            for j in (i + 1)..bodies.len() {
346                if aabb_overlap(&bodies[i], &bodies[j]) {
347                    brute.insert((i, j));
348                }
349            }
350        }
351
352        assert_eq!(sap, brute);
353        assert!(!brute.is_empty(), "test setup should produce some overlaps");
354    }
355
356    #[test]
357    fn sap_excludes_clearly_separated_pair() {
358        let bodies = vec![
359            Body::new(0, v(&[0.0, 0.0, 0.0]), v(&[0.0, 0.0, 0.0]), 1.0, 0.5).unwrap(),
360            Body::new(1, v(&[100.0, 100.0, 100.0]), v(&[0.0, 0.0, 0.0]), 1.0, 0.5).unwrap(),
361        ];
362        let pairs = sap_candidate_pairs(&bodies);
363        assert!(
364            pairs.is_empty(),
365            "far-apart, non-overlapping AABBs should not be a candidate pair: {pairs:?}"
366        );
367        assert!(!aabb_overlap(&bodies[0], &bodies[1]));
368    }
369}