1use glam::Vec2;
2
3use loam_math::{Bivector, Bivector2, EuclideanR2, Iso2};
4
5use crate::body::{BodyDef, RigidBody};
6use crate::collider::{Collider, ColliderKind};
7use crate::geometry::GeometryStore;
8use crate::integrator::{BroadphaseBound, PhysicsSpace};
9use crate::narrowphase::Narrowphase;
10use crate::response::Contact;
11
12fn cross2d(u: Vec2, v: Vec2) -> f32 {
13 u.x * v.y - u.y * v.x
14}
15
16fn inv_inertia(body: &RigidBody<EuclideanR2>) -> f32 {
17 if body.inv_mass() > 0.0 && body.inertia > 0.0 {
18 1.0 / body.inertia
19 } else {
20 0.0
21 }
22}
23
24impl PhysicsSpace for EuclideanR2 {
25 type AngVel = Bivector2;
26 type Inertia = f32;
27
28 fn broadphase_bound(&self) -> BroadphaseBound {
29 BroadphaseBound::Certified
30 }
31
32 fn supports_collider(&self, kind: ColliderKind) -> bool {
33 matches!(kind, ColliderKind::Sphere | ColliderKind::Polygon2D)
34 }
35
36 fn valid_vector(&self, vector: Vec2) -> bool {
37 vector.is_finite()
38 }
39
40 fn valid_orientation(&self, orientation: Iso2) -> bool {
41 orientation.rotation.a.is_finite()
42 && orientation.rotation.b.is_finite()
43 && orientation.translation.is_finite()
44 }
45
46 fn valid_angular_velocity(&self, angular_velocity: Bivector2) -> bool {
47 angular_velocity.0.is_finite()
48 }
49
50 fn valid_inertia(&self, inertia: f32) -> bool {
51 inertia.is_finite() && inertia >= 0.0 && (inertia == 0.0 || inertia.recip().is_finite())
52 }
53
54 fn integrate_orientation(&self, iso: Iso2, omega: Bivector2, dt: f32) -> Iso2 {
55 let delta = (omega * dt).exp();
56 Iso2 {
57 rotation: iso.rotation * delta,
58 translation: iso.translation,
59 }
60 }
61
62 fn apply_inv_inertia(&self, inertia: f32, torque: Bivector2) -> Bivector2 {
63 if inertia > 0.0 {
64 torque * (1.0 / inertia)
65 } else {
66 Bivector2::zero()
67 }
68 }
69
70 fn wedge(&self, a: Vec2, b: Vec2) -> Bivector2 {
71 Bivector2(cross2d(a, b))
72 }
73
74 fn velocity_at_point(&self, body: &RigidBody<EuclideanR2>, p: Vec2) -> Vec2 {
75 let r = p - body.position;
76 let w = body.angular_velocity.0;
77 body.velocity + Vec2::new(-w * r.y, w * r.x)
78 }
79
80 fn effective_mass_inv(
81 &self,
82 a: &RigidBody<EuclideanR2>,
83 b: &RigidBody<EuclideanR2>,
84 contact_point: Vec2,
85 direction: Vec2,
86 ) -> f32 {
87 let ra = contact_point - a.position;
88 let rb = contact_point - b.position;
89 let ra_cross = cross2d(ra, direction);
90 let rb_cross = cross2d(rb, direction);
91 a.inv_mass()
92 + b.inv_mass()
93 + ra_cross * ra_cross * inv_inertia(a)
94 + rb_cross * rb_cross * inv_inertia(b)
95 }
96
97 fn apply_contact_impulse(
98 &self,
99 a: &mut RigidBody<EuclideanR2>,
100 b: &mut RigidBody<EuclideanR2>,
101 contact_point: Vec2,
102 direction: Vec2,
103 magnitude: f32,
104 ) {
105 let ra = contact_point - a.position;
106 let rb = contact_point - b.position;
107 let lin = direction * magnitude;
108 a.velocity -= lin * a.inv_mass();
109 b.velocity += lin * b.inv_mass();
110
111 let inv_i_a = inv_inertia(a);
112 let inv_i_b = inv_inertia(b);
113 let torque_a = -cross2d(ra, lin);
114 let torque_b = cross2d(rb, lin);
115 a.angular_velocity = Bivector2(a.angular_velocity.0 + torque_a * inv_i_a);
116 b.angular_velocity = Bivector2(b.angular_velocity.0 + torque_b * inv_i_b);
117 }
118}
119
120pub fn disk_inertia(mass: f32, radius: f32) -> f32 {
121 0.5 * mass * radius * radius
122}
123
124pub fn sphere_body(
125 position: Vec2,
126 velocity: Vec2,
127 radius: f32,
128 mass: f32,
129) -> Option<BodyDef<EuclideanR2>> {
130 BodyDef::new(
131 position,
132 velocity,
133 Collider::sphere_at_origin(radius),
134 mass,
135 disk_inertia(mass, radius),
136 &EuclideanR2,
137 )
138}
139
140pub fn register_default_narrowphase(np: &mut Narrowphase<EuclideanR2>) {
142 np.register(ColliderKind::Sphere, ColliderKind::Sphere, sphere_sphere_r2);
143 np.register(
144 ColliderKind::Polygon2D,
145 ColliderKind::Polygon2D,
146 polygon_polygon_r2,
147 );
148 np.register(
149 ColliderKind::Sphere,
150 ColliderKind::Polygon2D,
151 sphere_polygon_r2,
152 );
153}
154
155fn sphere_sphere_r2(
156 a: &RigidBody<EuclideanR2>,
157 b: &RigidBody<EuclideanR2>,
158 geometry: &GeometryStore,
159 space: &EuclideanR2,
160) -> Option<Contact<EuclideanR2>> {
161 let Some(&Collider::Sphere { radius: ra, .. }) = geometry.get(a.collider()) else {
162 return None;
163 };
164 let Some(&Collider::Sphere { radius: rb, .. }) = geometry.get(b.collider()) else {
165 return None;
166 };
167
168 use loam_math::Space;
169 let d = space.distance(a.position, b.position);
170 let combined = ra + rb;
171 if d >= combined {
172 return None;
173 }
174 let log = space.log(a.position, b.position);
175 let len = log.length();
176 let normal = if len > 1e-8 { log / len } else { Vec2::Y };
177
178 let surface_a = a.position + normal * ra;
179 let surface_b = b.position - normal * rb;
180 let point = (surface_a + surface_b) * 0.5;
181
182 Some(Contact {
183 normal,
184 point,
185 penetration: combined - d,
186 restitution: (a.restitution + b.restitution) * 0.5,
187 })
188}
189
190use loam_math::Rotor;
191
192struct WorldPolygon<'a> {
193 local: &'a [Vec2],
194 position: Vec2,
195 rotation: loam_math::Rotor2,
196}
197
198impl WorldPolygon<'_> {
199 fn len(&self) -> usize {
200 self.local.len()
201 }
202 fn vertex(&self, i: usize) -> Vec2 {
203 self.rotation.apply(self.local[i]) + self.position
204 }
205 fn vertices(&self) -> impl Iterator<Item = Vec2> + '_ {
206 (0..self.len()).map(|i| self.vertex(i))
207 }
208}
209
210fn project_onto(axis: Vec2, verts: &WorldPolygon<'_>) -> (f32, f32) {
211 let first = verts.vertex(0).dot(axis);
212 let (mut lo, mut hi) = (first, first);
213 for v in verts.vertices().skip(1) {
214 let p = v.dot(axis);
215 if p < lo {
216 lo = p;
217 }
218 if p > hi {
219 hi = p;
220 }
221 }
222 (lo, hi)
223}
224
225fn best_axis_from(sides: &WorldPolygon<'_>, other: &WorldPolygon<'_>) -> Option<(Vec2, f32)> {
226 let n = sides.len();
227 let mut best: Option<(Vec2, f32)> = None;
228 for i in 0..n {
229 let v0 = sides.vertex(i);
230 let v1 = sides.vertex((i + 1) % n);
231 let edge = v1 - v0;
232 let len = edge.length();
233 if len < 1e-8 {
234 continue;
235 }
236 let normal = Vec2::new(edge.y, -edge.x) / len;
237 let (lo_a, hi_a) = project_onto(normal, sides);
238 let (lo_b, hi_b) = project_onto(normal, other);
239 let positive = hi_a - lo_b;
240 let negative = hi_b - lo_a;
241 if positive <= 0.0 || negative <= 0.0 {
242 return None;
243 }
244 let (normal, overlap) = if positive <= negative {
245 (normal, positive)
246 } else {
247 (-normal, negative)
248 };
249 match best {
250 None => best = Some((normal, overlap)),
251 Some((_, prev)) if overlap < prev => best = Some((normal, overlap)),
252 _ => {}
253 }
254 }
255 best
256}
257
258fn polygon_polygon_r2(
259 a: &RigidBody<EuclideanR2>,
260 b: &RigidBody<EuclideanR2>,
261 geometry: &GeometryStore,
262 _space: &EuclideanR2,
263) -> Option<Contact<EuclideanR2>> {
264 let Some(Collider::Polygon2D { vertices: a_local }) = geometry.get(a.collider()) else {
265 return None;
266 };
267 let Some(Collider::Polygon2D { vertices: b_local }) = geometry.get(b.collider()) else {
268 return None;
269 };
270 if a_local.len() < 3 || b_local.len() < 3 {
271 return None;
272 }
273
274 let va = WorldPolygon {
275 local: a_local,
276 position: a.position,
277 rotation: a.orientation.rotation,
278 };
279 let vb = WorldPolygon {
280 local: b_local,
281 position: b.position,
282 rotation: b.orientation.rotation,
283 };
284
285 let mut best = best_axis_from(&va, &vb)?;
286 if let Some((n, o)) = best_axis_from(&vb, &va) {
287 if o < best.1 {
288 best = (-n, o);
289 }
290 } else {
291 return None;
292 }
293
294 let (normal, penetration) = best;
295
296 let mut deepest_a = va.vertex(0);
297 let mut max_proj = va.vertex(0).dot(normal);
298 for v in va.vertices().skip(1) {
299 let p = v.dot(normal);
300 if p > max_proj {
301 max_proj = p;
302 deepest_a = v;
303 }
304 }
305 let mut deepest_b = vb.vertex(0);
306 let mut min_proj = vb.vertex(0).dot(normal);
307 for v in vb.vertices().skip(1) {
308 let p = v.dot(normal);
309 if p < min_proj {
310 min_proj = p;
311 deepest_b = v;
312 }
313 }
314
315 let a_inside_b = point_in_convex_ccw(&vb, deepest_a);
316 let b_inside_a = point_in_convex_ccw(&va, deepest_b);
317 let point = match (a_inside_b, b_inside_a) {
318 (true, false) => deepest_a,
319 (false, true) => deepest_b,
320 _ => (deepest_a + deepest_b) * 0.5,
321 };
322
323 Some(Contact {
324 normal,
325 point,
326 penetration,
327 restitution: (a.restitution + b.restitution) * 0.5,
328 })
329}
330
331fn point_in_convex_ccw(poly: &WorldPolygon<'_>, p: Vec2) -> bool {
332 for i in 0..poly.len() {
333 let v0 = poly.vertex(i);
334 let v1 = poly.vertex((i + 1) % poly.len());
335 let edge = v1 - v0;
336 let outward = Vec2::new(edge.y, -edge.x);
337 if (p - v0).dot(outward) > 0.0 {
338 return false;
339 }
340 }
341 true
342}
343
344fn closest_on_polygon_boundary(poly: &WorldPolygon<'_>, p: Vec2) -> Option<(Vec2, f32, Vec2)> {
345 let mut best: Option<(Vec2, f32, Vec2)> = None;
346 for i in 0..poly.len() {
347 let v0 = poly.vertex(i);
348 let v1 = poly.vertex((i + 1) % poly.len());
349 let edge = v1 - v0;
350 let len = edge.length();
351 if len < 1e-6 {
352 continue;
353 }
354 let t = ((p - v0).dot(edge) / (len * len)).clamp(0.0, 1.0);
355 let q = v0 + edge * t;
356 let d2 = (q - p).length_squared();
357 if best.is_none_or(|(_, best_d2, _)| d2 < best_d2) {
358 best = Some((q, d2, Vec2::new(edge.y, -edge.x) / len));
359 }
360 }
361 best.map(|(q, d2, outward)| (q, d2.sqrt(), outward))
362}
363
364fn sphere_polygon_r2(
365 a: &RigidBody<EuclideanR2>,
366 b: &RigidBody<EuclideanR2>,
367 geometry: &GeometryStore,
368 _space: &EuclideanR2,
369) -> Option<Contact<EuclideanR2>> {
370 let Some(&Collider::Sphere { radius, .. }) = geometry.get(a.collider()) else {
371 return None;
372 };
373 let Some(Collider::Polygon2D { vertices: b_local }) = geometry.get(b.collider()) else {
374 return None;
375 };
376 if b_local.len() < 3 {
377 return None;
378 }
379
380 let vb = WorldPolygon {
381 local: b_local,
382 position: b.position,
383 rotation: b.orientation.rotation,
384 };
385 let center = a.position;
386 let (closest, dist, edge_outward) = closest_on_polygon_boundary(&vb, center)?;
387 let inside = point_in_convex_ccw(&vb, center);
388
389 if !inside && dist >= radius {
390 return None;
391 }
392
393 let out_of_polygon = if inside {
394 closest - center
395 } else {
396 center - closest
397 };
398 let normal = -out_of_polygon.try_normalize().unwrap_or(edge_outward);
399
400 let penetration = if inside { radius + dist } else { radius - dist };
401
402 Some(Contact {
403 normal,
404 point: closest,
405 penetration,
406 restitution: (a.restitution + b.restitution) * 0.5,
407 })
408}
409
410pub fn regular_polygon_vertices(n: u32, r: f32) -> Vec<Vec2> {
412 use std::f32::consts::TAU;
413 (0..n)
414 .map(|k| {
415 let theta = TAU * (k as f32) / (n as f32);
416 Vec2::new(theta.cos(), theta.sin()) * r
417 })
418 .collect()
419}
420
421pub fn regular_polygon_inertia(mass: f32, n: u32, r: f32) -> f32 {
422 use std::f32::consts::PI;
423 let c = (PI / n as f32).cos();
424 (mass * r * r / 6.0) * (1.0 + 2.0 * c * c)
425}
426
427pub fn polygon_body(
428 position: Vec2,
429 velocity: Vec2,
430 n: u32,
431 circumradius: f32,
432 mass: f32,
433) -> Option<BodyDef<EuclideanR2>> {
434 BodyDef::new(
435 position,
436 velocity,
437 Collider::Polygon2D {
438 vertices: regular_polygon_vertices(n, circumradius),
439 },
440 mass,
441 regular_polygon_inertia(mass, n, circumradius),
442 &EuclideanR2,
443 )
444}
445
446fn rectangle_vertices(half_extents: Vec2) -> Vec<Vec2> {
447 let (hx, hy) = (half_extents.x, half_extents.y);
448 vec![
449 Vec2::new(hx, -hy),
450 Vec2::new(hx, hy),
451 Vec2::new(-hx, hy),
452 Vec2::new(-hx, -hy),
453 ]
454}
455
456pub fn rectangle_body(
457 center: Vec2,
458 velocity: Vec2,
459 half_extents: Vec2,
460 mass: f32,
461) -> Option<BodyDef<EuclideanR2>> {
462 let inertia = mass * (half_extents.x * half_extents.x + half_extents.y * half_extents.y) / 3.0;
463 BodyDef::new(
464 center,
465 velocity,
466 Collider::Polygon2D {
467 vertices: rectangle_vertices(half_extents),
468 },
469 mass,
470 inertia,
471 &EuclideanR2,
472 )
473}
474
475pub fn static_wall(center: Vec2, half_extents: Vec2) -> Option<BodyDef<EuclideanR2>> {
476 BodyDef::fixed(
477 center,
478 Collider::Polygon2D {
479 vertices: rectangle_vertices(half_extents),
480 },
481 1.0,
482 &EuclideanR2,
483 )
484}
485
486#[cfg(test)]
487mod tests {
488 use super::*;
489
490 use crate::world::World;
491
492 #[test]
493 fn static_body_ignores_gravity() {
494 let mut world = World::new(EuclideanR2);
495 let id = world.push_body(
496 BodyDef::fixed(
497 Vec2::new(0.0, 0.0),
498 Collider::sphere_at_origin(1.0),
499 disk_inertia(0.0, 1.0),
500 &EuclideanR2,
501 )
502 .unwrap(),
503 );
504 world.set_gravity(Some(Vec2::new(0.0, -9.8))).unwrap();
505
506 for _ in 0..10 {
507 world.step(1.0 / 60.0).unwrap();
508 }
509
510 let body = &world.bodies[id];
511 assert_eq!(body.position, Vec2::ZERO);
512 assert_eq!(body.velocity, Vec2::ZERO);
513 }
514
515 #[test]
516 fn sphere_sphere_contact_detected() {
517 let mut np = Narrowphase::<EuclideanR2>::new();
518 register_default_narrowphase(&mut np);
519 let mut g = GeometryStore::default();
520
521 let a = row(
522 sphere_body(Vec2::ZERO, Vec2::ZERO, 1.0, 1.0).unwrap(),
523 &mut g,
524 );
525 let b = row(
526 sphere_body(Vec2::new(1.5, 0.0), Vec2::ZERO, 1.0, 1.0).unwrap(),
527 &mut g,
528 );
529 let contact = np.test(&a, &b, &g, &EuclideanR2).expect("should collide");
530 assert!((contact.normal - Vec2::X).length() < 1e-5);
531 assert!((contact.penetration - 0.5).abs() < 1e-5);
532 }
533
534 #[test]
535 fn regular_polygon_vertices_are_ccw() {
536 let verts = regular_polygon_vertices(4, 1.0);
537 assert_eq!(verts.len(), 4);
538 for i in 0..4 {
539 let e0 = verts[(i + 1) % 4] - verts[i];
540 let e1 = verts[(i + 2) % 4] - verts[(i + 1) % 4];
541 let cross = e0.x * e1.y - e0.y * e1.x;
542 assert!(cross > 0.0, "winding not CCW at edge {i}: cross={cross}");
543 }
544 }
545
546 fn aa_box(center: Vec2, half: Vec2, mass: f32) -> BodyDef<EuclideanR2> {
547 rectangle_body(center, Vec2::ZERO, half, mass).unwrap()
548 }
549
550 fn row(def: BodyDef<EuclideanR2>, geometry: &mut GeometryStore) -> RigidBody<EuclideanR2> {
551 def.into_row(geometry, &EuclideanR2)
552 }
553
554 #[test]
555 fn contained_polygon_uses_exit_distance_and_reverses_with_pair() {
556 let mut g = GeometryStore::default();
557 let outer = row(aa_box(Vec2::ZERO, Vec2::splat(2.0), 1.0), &mut g);
558 let inner = row(aa_box(Vec2::new(0.5, 0.0), Vec2::splat(0.5), 1.0), &mut g);
559 let forward = polygon_polygon_r2(&outer, &inner, &g, &EuclideanR2).unwrap();
560 let reverse = polygon_polygon_r2(&inner, &outer, &g, &EuclideanR2).unwrap();
561 assert_eq!(forward.penetration, 2.0);
562 assert_eq!(reverse.penetration, 2.0);
563 assert_eq!(forward.normal, Vec2::X);
564 assert_eq!(reverse.normal, -Vec2::X);
565 }
566
567 #[test]
568 fn polygon_polygon_detects_overlap() {
569 let mut np = Narrowphase::<EuclideanR2>::new();
570 register_default_narrowphase(&mut np);
571 let mut g = GeometryStore::default();
572
573 let a = row(aa_box(Vec2::ZERO, Vec2::ONE, 1.0), &mut g);
574 let b = row(aa_box(Vec2::new(1.5, 0.0), Vec2::ONE, 1.0), &mut g);
575
576 let c = np.test(&a, &b, &g, &EuclideanR2).expect("should collide");
577 assert!(
578 c.normal.dot(Vec2::X).abs() > 0.99,
579 "normal not ±X: {:?}",
580 c.normal
581 );
582 assert!(
583 c.normal.dot(Vec2::X) > 0.0,
584 "normal not A->B: {:?}",
585 c.normal
586 );
587 assert!(
588 (c.penetration - 0.5).abs() < 1e-4,
589 "penetration: {}",
590 c.penetration
591 );
592 }
593
594 #[test]
595 fn polygon_rotation_affects_collision() {
596 let mut np = Narrowphase::<EuclideanR2>::new();
597 register_default_narrowphase(&mut np);
598 let mut g = GeometryStore::default();
599
600 let a = row(
601 polygon_body(Vec2::ZERO, Vec2::ZERO, 4, 1.0, 1.0).unwrap(),
602 &mut g,
603 );
604 let b = row(
605 polygon_body(Vec2::new(1.9, 0.0), Vec2::ZERO, 4, 1.0, 1.0).unwrap(),
606 &mut g,
607 );
608 assert!(
609 np.test(&a, &b, &g, &EuclideanR2).is_some(),
610 "unrotated squares at 1.9 should overlap"
611 );
612
613 let mut b = row(
614 polygon_body(Vec2::new(1.9, 0.0), Vec2::ZERO, 4, 1.0, 1.0).unwrap(),
615 &mut g,
616 );
617 b.orientation = Iso2 {
618 rotation: loam_math::Bivector2(std::f32::consts::FRAC_PI_4).exp(),
619 translation: Vec2::ZERO,
620 };
621 assert!(np.test(&a, &b, &g, &EuclideanR2).is_none());
622 }
623
624 #[test]
625 fn sphere_polygon_edge_contact() {
626 let mut np = Narrowphase::<EuclideanR2>::new();
627 register_default_narrowphase(&mut np);
628 let mut g = GeometryStore::default();
629
630 let square = row(
631 polygon_body(Vec2::ZERO, Vec2::ZERO, 4, 1.0, 1.0).unwrap(),
632 &mut g,
633 );
634 let sphere = row(
635 sphere_body(Vec2::new(1.3, 0.0), Vec2::ZERO, 0.5, 1.0).unwrap(),
636 &mut g,
637 );
638
639 let c = np
640 .test(&sphere, &square, &g, &EuclideanR2)
641 .expect("should collide");
642 assert!(c.normal.dot(-Vec2::X) > 0.99, "normal: {:?}", c.normal);
643 assert!(
644 (c.penetration - 0.2).abs() < 1e-4,
645 "penetration: {}",
646 c.penetration
647 );
648 }
649
650 const SLAB_HALF: f32 = 0.05;
651 const DISK_RADIUS: f32 = 0.1;
652
653 fn slab_and_disk(
654 center_x: f32,
655 geometry: &mut GeometryStore,
656 ) -> (RigidBody<EuclideanR2>, RigidBody<EuclideanR2>) {
657 (
658 row(
659 sphere_body(Vec2::new(center_x, 0.0), Vec2::ZERO, DISK_RADIUS, 1.0).unwrap(),
660 geometry,
661 ),
662 row(
663 static_wall(Vec2::ZERO, Vec2::new(SLAB_HALF, 2.0)).unwrap(),
664 geometry,
665 ),
666 )
667 }
668
669 #[test]
670 fn sphere_polygon_normal_leaves_through_the_nearest_face_from_inside() {
671 let mut np = Narrowphase::<EuclideanR2>::new();
672 register_default_narrowphase(&mut np);
673 let mut g = GeometryStore::default();
674
675 for (center_x, exit) in [(-0.03, -Vec2::X), (0.03, Vec2::X)] {
676 let (disk, wall) = slab_and_disk(center_x, &mut g);
677 let c = np
678 .test(&disk, &wall, &g, &EuclideanR2)
679 .expect("centre is inside");
680 assert!(
681 (-c.normal).dot(exit) > 0.999,
682 "disk at x = {center_x} leaves along {:?}, not {exit:?}",
683 -c.normal
684 );
685 assert!(
686 (c.penetration - (DISK_RADIUS + SLAB_HALF - center_x.abs())).abs() < 1e-5,
687 "depth {} is not the run to the near face plus the radius",
688 c.penetration
689 );
690 }
691 }
692
693 #[test]
694 fn sphere_polygon_contact_is_continuous_across_the_face() {
695 let mut np = Narrowphase::<EuclideanR2>::new();
696 register_default_narrowphase(&mut np);
697 let mut g = GeometryStore::default();
698
699 const STEP: f32 = 1e-4;
700 let (outside, wall) = slab_and_disk(-SLAB_HALF - STEP, &mut g);
701 let (inside, _) = slab_and_disk(-SLAB_HALF + STEP, &mut g);
702 let out = np
703 .test(&outside, &wall, &g, &EuclideanR2)
704 .expect("overlapping");
705 let inn = np
706 .test(&inside, &wall, &g, &EuclideanR2)
707 .expect("overlapping");
708
709 assert!(
710 (out.normal - inn.normal).length() < 1e-3,
711 "normal jumps from {:?} to {:?} across the face",
712 out.normal,
713 inn.normal
714 );
715 assert!(
716 (inn.penetration - out.penetration - 2.0 * STEP).abs() < 1e-5,
717 "depth jumps from {} to {} over a {} crossing",
718 out.penetration,
719 inn.penetration,
720 2.0 * STEP
721 );
722 }
723
724 #[test]
725 fn sphere_polygon_centre_on_the_face_leaves_along_that_face_normal() {
726 let mut np = Narrowphase::<EuclideanR2>::new();
727 register_default_narrowphase(&mut np);
728 let mut g = GeometryStore::default();
729
730 let (disk, wall) = slab_and_disk(-SLAB_HALF, &mut g);
731 let c = np
732 .test(&disk, &wall, &g, &EuclideanR2)
733 .expect("touching the face");
734 assert!(
735 (-c.normal).dot(-Vec2::X) > 0.999,
736 "leaves along {:?}, not −x̂",
737 -c.normal
738 );
739 assert!((c.penetration - DISK_RADIUS).abs() < 1e-5);
740 }
741
742 #[test]
743 fn sphere_polygon_grazing_at_exactly_the_radius_reports_no_contact() {
744 let mut np = Narrowphase::<EuclideanR2>::new();
745 register_default_narrowphase(&mut np);
746 let mut g = GeometryStore::default();
747
748 const STEP: f32 = 1e-5;
749 let graze = -(SLAB_HALF + DISK_RADIUS);
750 for (center_x, expected) in [
751 (graze - STEP, None),
752 (graze, None),
753 (graze + STEP, Some(STEP)),
754 ] {
755 let (disk, wall) = slab_and_disk(center_x, &mut g);
756 match (np.test(&disk, &wall, &g, &EuclideanR2), expected) {
757 (None, None) => {}
758 (Some(c), Some(depth)) => {
759 assert!(
760 (c.penetration - depth).abs() < 1e-6,
761 "depth {} at x = {center_x} is not the {depth} of overlap",
762 c.penetration
763 );
764 assert!((-c.normal).dot(-Vec2::X) > 0.999);
765 }
766 (got, _) => panic!("x = {center_x} reported {:?}", got.map(|c| c.penetration)),
767 }
768 }
769 }
770
771 #[test]
772 fn sphere_sphere_grazing_at_exactly_the_combined_radius_reports_no_contact() {
773 let mut np = Narrowphase::<EuclideanR2>::new();
774 register_default_narrowphase(&mut np);
775 let mut g = GeometryStore::default();
776
777 const STEP: f32 = 1e-5;
778 let a = row(
779 sphere_body(Vec2::ZERO, Vec2::ZERO, 0.5, 1.0).unwrap(),
780 &mut g,
781 );
782 for (gap, expected) in [(STEP, None), (0.0, None), (-STEP, Some(STEP))] {
783 let b = row(
784 sphere_body(Vec2::new(1.0 + gap, 0.0), Vec2::ZERO, 0.5, 1.0).unwrap(),
785 &mut g,
786 );
787 match (np.test(&a, &b, &g, &EuclideanR2), expected) {
788 (None, None) => {}
789 (Some(c), Some(depth)) => assert!(
790 (c.penetration - depth).abs() < 1e-6,
791 "depth {} at gap {gap} is not {depth}",
792 c.penetration
793 ),
794 (got, _) => panic!("gap {gap} reported {:?}", got.map(|c| c.penetration)),
795 }
796 }
797 }
798
799 #[test]
800 fn polygon_body_rejects_collinear_or_clockwise_boundaries() {
801 for vertices in [vec![Vec2::ZERO; 4], vec![Vec2::ZERO, Vec2::Y, Vec2::X]] {
802 assert!(BodyDef::fixed(
803 Vec2::ZERO,
804 Collider::Polygon2D { vertices },
805 1.0,
806 &EuclideanR2
807 )
808 .is_none());
809 }
810 }
811
812 #[test]
813 fn sphere_polygon_reverse_pair_handled() {
814 let mut np = Narrowphase::<EuclideanR2>::new();
815 register_default_narrowphase(&mut np);
816 let mut g = GeometryStore::default();
817
818 let square = row(
819 polygon_body(Vec2::ZERO, Vec2::ZERO, 4, 1.0, 1.0).unwrap(),
820 &mut g,
821 );
822 let sphere = row(
823 sphere_body(Vec2::new(1.3, 0.0), Vec2::ZERO, 0.5, 1.0).unwrap(),
824 &mut g,
825 );
826
827 let c = np
828 .test(&square, &sphere, &g, &EuclideanR2)
829 .expect("should collide");
830 assert!(c.normal.dot(Vec2::X) > 0.99, "normal: {:?}", c.normal);
831 }
832
833 #[test]
834 fn off_center_impact_produces_angular_velocity() {
835 let mut world = World::new(EuclideanR2);
836 register_default_narrowphase(&mut world.narrowphase);
837
838 let square_id = world.push_body(aa_box(Vec2::ZERO, Vec2::ONE, 1.0));
839 let _sphere_id = world
840 .push_body(sphere_body(Vec2::new(0.6, 3.0), Vec2::new(0.0, -10.0), 0.3, 1.0).unwrap());
841
842 for _ in 0..60 {
843 world.step(1.0 / 120.0).unwrap();
844 }
845
846 let omega = world.bodies[square_id].angular_velocity.0;
847 assert!(
848 omega < -0.05,
849 "square failed to rotate clockwise from off-center hit: ω = {omega}"
850 );
851 }
852
853 #[test]
854 fn falling_polygons_reach_the_floor_without_crossing_it() {
855 let mut world = World::new(EuclideanR2);
856 register_default_narrowphase(&mut world.narrowphase);
857
858 let floor_top = 0.5;
859 world.push_body(static_wall(Vec2::new(0.0, 0.0), Vec2::new(10.0, 0.5)).unwrap());
860
861 for i in 0..5 {
862 let x = -2.0 + i as f32 * 1.0;
863 world.push_body(
864 polygon_body(Vec2::new(x, 4.0 + i as f32 * 0.2), Vec2::ZERO, 6, 0.4, 1.0).unwrap(),
865 );
866 }
867
868 world.set_gravity(Some(Vec2::new(0.0, -9.8))).unwrap();
869
870 for _ in 0..240 {
871 world.step(1.0 / 60.0).unwrap();
872 }
873
874 for (idx, body) in world.bodies.iter().enumerate().skip(1) {
875 let Some(Collider::Polygon2D { vertices }) = world.collider(body) else {
876 panic!("polygon fixture")
877 };
878 let lowest = vertices
879 .iter()
880 .map(|&v| (body.orientation.rotation.apply(v) + body.position).y)
881 .fold(f32::INFINITY, f32::min);
882 assert!(
883 (floor_top - 0.05..=floor_top + 0.05).contains(&lowest),
884 "body {idx}: lowest={lowest}"
885 );
886 }
887 }
888
889 #[test]
890 fn head_on_collision_separates_spheres() {
891 let mut world = World::new(EuclideanR2);
892 register_default_narrowphase(&mut world.narrowphase);
893
894 world.push_body(sphere_body(Vec2::new(-1.0, 0.0), Vec2::new(2.0, 0.0), 0.5, 1.0).unwrap());
895 world.push_body(sphere_body(Vec2::new(1.0, 0.0), Vec2::new(-2.0, 0.0), 0.5, 1.0).unwrap());
896
897 for _ in 0..30 {
898 world.step(1.0 / 60.0).unwrap();
899 }
900
901 for body in world.bodies.iter() {
902 assert!(body.angular_velocity.0.abs() < 1e-3);
903 }
904 assert!(
905 world.bodies[0].velocity.x < 0.0,
906 "body 0 should bounce back"
907 );
908 assert!(
909 world.bodies[1].velocity.x > 0.0,
910 "body 1 should bounce back"
911 );
912 }
913
914 #[test]
915 fn box_stack_settles_to_rest() {
916 let mut world = World::new(EuclideanR2);
917 register_default_narrowphase(&mut world.narrowphase);
918
919 let floor_top = 0.5;
920 world.push_body(static_wall(Vec2::new(0.0, 0.0), Vec2::new(8.0, 0.5)).unwrap());
921
922 const N: usize = 3;
923 const HALF: f32 = 0.5;
924 for i in 0..N {
925 let y = floor_top + HALF + i as f32 * (2.0 * HALF + 0.05);
926 let id = world.push_body(aa_box(Vec2::new(0.0, y), Vec2::splat(HALF), 1.0));
927 world.bodies[id].restitution = 0.0;
928 }
929 world.set_solver_iterations(16);
930
931 world.set_gravity(Some(Vec2::new(0.0, -9.8))).unwrap();
932
933 for _ in 0..300 {
934 world.step(1.0 / 60.0).unwrap();
935 }
936
937 for (idx, body) in world.bodies.iter().enumerate().skip(1) {
938 assert!(
939 body.position.is_finite() && body.velocity.is_finite(),
940 "body {idx} not finite"
941 );
942 assert!(
943 body.velocity.length() < 0.5,
944 "body {idx} still moving: |v| = {}",
945 body.velocity.length()
946 );
947 assert!(
948 body.angular_velocity.0.abs() < 1.0,
949 "body {idx} still spinning: ω = {}",
950 body.angular_velocity.0
951 );
952 assert!(
953 body.position.x.abs() < 1.0,
954 "body {idx} drifted off stack: x = {}",
955 body.position.x
956 );
957 assert!(
958 body.position.y - HALF >= floor_top - 0.1,
959 "body {idx} sank into floor: y_bottom = {}",
960 body.position.y - HALF
961 );
962 }
963 }
964}