From 899d9bd864280422e5a8a4be1340d582c1cf42bc Mon Sep 17 00:00:00 2001
From: notgiven688 <notgiven688@users.noreply.github.com>
Date: Mon, 1 Apr 2024 13:36:42 +0200
Subject: [PATCH] Make EPA more stable by skipping origin enclosed test once
 the origin has been enclosed

---
 src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs b/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs
index 24181204..d8fca110 100644
--- a/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs
+++ b/src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs
@@ -293,6 +293,10 @@ public ref Triangle GetClosestTriangle()
         int closestIndex = -1;
         float currentMin = float.MaxValue;
 
+        // We can skip the test for enclosed origin if the origin was
+        // already enclosed once.
+        bool skipTest = originEnclosed;
+
         originEnclosed = true;
 
         for (int i = 0; i < tPointer; i++)
@@ -303,7 +307,7 @@ public ref Triangle GetClosestTriangle()
                 closestIndex = i;
             }
 
-            if (!triangles[i].FacingOrigin) originEnclosed = false;
+            if (!triangles[i].FacingOrigin) originEnclosed = skipTest;
         }
 
         return ref triangles[closestIndex];