From 7e28a3b88efa5322bf39366ddf3617f6c2c77ff1 Mon Sep 17 00:00:00 2001
From: Joona Aalto <jondolf.dev@gmail.com>
Date: Mon, 11 Nov 2024 22:33:07 +0200
Subject: [PATCH] Add `find_deepest_contact` for `Contacts` and
 `ContactManifold`

---
 src/collision/mod.rs | 31 +++++++++++++++++++++++++++++++
 1 file changed, 31 insertions(+)

diff --git a/src/collision/mod.rs b/src/collision/mod.rs
index ec5fa007..4e562ad5 100644
--- a/src/collision/mod.rs
+++ b/src/collision/mod.rs
@@ -340,6 +340,23 @@ impl Contacts {
     pub fn collision_stopped(&self) -> bool {
         self.during_previous_frame && !self.during_current_frame
     }
+
+    /// Returns the contact with the largest penetration depth.
+    ///
+    /// If the objects are separated but there is still a speculative contact,
+    /// the penetration depth will be negative.
+    ///
+    /// If there are no contacts, `None` is returned.
+    pub fn find_deepest_contact(&self) -> Option<&ContactData> {
+        self.manifolds
+            .iter()
+            .filter_map(|manifold| manifold.find_deepest_contact())
+            .max_by(|a, b| {
+                a.penetration
+                    .partial_cmp(&b.penetration)
+                    .unwrap_or(std::cmp::Ordering::Equal)
+            })
+    }
 }
 
 /// A contact manifold between two colliders, containing a set of contact points.
@@ -420,6 +437,20 @@ impl ContactManifold {
             }
         }
     }
+
+    /// Returns the contact with the largest penetration depth.
+    ///
+    /// If the objects are separated but there is still a speculative contact,
+    /// the penetration depth will be negative.
+    ///
+    /// If there are no contacts, `None` is returned.
+    pub fn find_deepest_contact(&self) -> Option<&ContactData> {
+        self.contacts.iter().max_by(|a, b| {
+            a.penetration
+                .partial_cmp(&b.penetration)
+                .unwrap_or(std::cmp::Ordering::Equal)
+        })
+    }
 }
 
 /// Data related to a single contact between two bodies.