1#![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 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 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 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 assert_abs_diff_eq!(b.inertia, 0.2, epsilon = 1e-12);
201 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]); let dt = 0.01_f64;
212 let theta = 1.0 * dt; b.spin(dt);
214 let r = b.orientation_matrix();
215 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 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]; let r = quat_to_matrix(&q);
231 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 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); 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 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 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); let rel = b.velocity - a.velocity;
283 rel[1].abs() }
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 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); 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 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) }
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}