Revert "Use ClosestToOriginSq < NumericEpsilon as termination condition"

This reverts commit 8f83886.

src/Jitter2/Collision/NarrowPhase/ConvexPolytope.cs

@@ -110,7 +110,7 @@ public static bool Equals(in Edge a, in Edge b)
     public readonly bool OriginEnclosed => originEnclosed;
 
     /// <summary>
-    /// Computes the barycentric coordinates of the origin projected onto a given triangle.
+    /// Computes the barycentric coordinates of the origin projected onto a given triangle. 
     /// These coordinates are used to retrieve points in A- and B-space.
     /// </summary>
     public void CalculatePoints(in Triangle ctri, out JVector pA, out JVector pB)
@@ -274,7 +274,7 @@ private bool CreateTriangle(short a, short b, short c)
     }
 
     /// <summary>
-    /// Iterates through all triangles of the convex polytope and returns the one closest
+    /// Iterates through all triangles of the convex polytope and returns the one closest 
     /// to the origin (0, 0, 0), based on the minimum distance.
     /// </summary>
     public ref Triangle GetClosestTriangle()
@@ -352,7 +352,7 @@ public void InitHeap()
     }
 
     /// <summary>
-    /// Incorporates a single point into the polyhedron, disregarding A- and B-space.
+    /// Incorporates a single point into the polyhedron, disregarding A- and B-space. 
     /// This operation contrasts with <see cref="AddVertex"/>.
     /// </summary>
     /// <returns>Indicates whether the polyhedron successfully incorporated the new point.</returns>
@@ -364,8 +364,8 @@ public bool AddPoint(in JVector point)
     }
 
     /// <summary>
-    /// Adds a vertex to the polyhedron. Note: This operation invalidates the reference
-    /// returned by previous calls to <see cref="GetClosestTriangle"/>, regardless of
+    /// Adds a vertex to the polyhedron. Note: This operation invalidates the reference 
+    /// returned by previous calls to <see cref="GetClosestTriangle"/>, regardless of 
     /// the return value of this method.
     /// </summary>
     /// <returns>Indicates whether the polyhedron successfully incorporated the new vertex.</returns>
@@ -380,11 +380,13 @@ public bool AddVertex(in Vertex vertex)
         for (int index = tCount; index-- > 0;)
         {
             if (!IsLit(index, vPointer)) continue;
+            Edge edge;
+            bool added;
 
             for (int k = 0; k < 3; k++)
             {
-                var edge = new Edge(Triangles[index][(k + 0) % 3], Triangles[index][(k + 1) % 3]);
-                var added = true;
+                edge = new Edge(Triangles[index][(k + 0) % 3], Triangles[index][(k + 1) % 3]);
+                added = true;
                 for (int e = ePointer; e-- > 0;)
                 {
                     if (Edge.Equals(edges[e], edge))

src/Jitter2/Collision/NarrowPhase/NarrowPhase.cs

@@ -40,7 +40,7 @@ private unsafe struct Solver
     {
         public MinkowskiDifference MKD;
         public ConvexPolytope ConvexPolytope;
-
+        
         public bool PointTest(in JVector origin)
         {
             const float CollideEpsilon = 1e-4f;
@@ -94,7 +94,7 @@ public bool RayCast(in JVector origin, in JVector direction, out float fraction,
 
             JVector r = direction;
             JVector x = origin;
-
+            
             var center = MKD.SupportA.GeometricCenter;
             JVector v = x - center;
 
@@ -140,7 +140,7 @@ public bool RayCast(in JVector origin, in JVector direction, out float fraction,
             fraction = lambda;
 
             float nlen2 = normal.LengthSquared();
-
+            
             if (nlen2 > NumericEpsilon)
             {
                 normal *= 1.0f / MathF.Sqrt(nlen2);
@@ -170,7 +170,7 @@ private bool SolveMPREPA(ref JVector point1, ref JVector point2, ref JVector nor
 
                 if (ctri.ClosestToOriginSq < NumericEpsilon)
                 {
-                    goto converged;
+                    searchDir = ctri.Normal;
                 }
 
                 MKD.Support(searchDir, out ConvexPolytope.Vertex vertex);
@@ -468,7 +468,7 @@ public bool SolveGJKEPA(out JVector point1, out JVector point2, out JVector norm
 
                 if (ctri.ClosestToOriginSq < NumericEpsilon)
                 {
-                    goto converged;
+                    searchDir = ctri.Normal;
                 }
 
                 MKD.Support(searchDir, out ConvexPolytope.Vertex vertex);
@@ -520,7 +520,7 @@ public bool SolveGJKEPA(out JVector point1, out JVector point2, out JVector norm
 
     // ------------------------------------------------------------------------------------------------------------
     [ThreadStatic] private static Solver solver;
-
+    
     /// <summary>
     /// Check if a point is inside a shape.
     /// </summary>
@@ -550,7 +550,7 @@ public static bool PointTest(ISupportMap support, in JMatrix orientation,
 
         solver.MKD.SupportA = support;
         solver.MKD.SupportB = null!;
-
+        
         return solver.PointTest(transformedOrigin);
     }
 
@@ -714,9 +714,9 @@ public static bool MPREPA(ISupportMap supportA, ISupportMap supportB,
     /// <summary>
     /// Detects whether two convex shapes overlap and provides detailed collision information.
     /// It assumes that support shape A is at position zero and not rotated.
-    /// Internally, this method utilizes the Minkowski Portal Refinement (MPR) to obtain the
+    /// Internally, this method utilizes the Minkowski Portal Refinement (MPR) to obtain the 
     /// Although MPR is not exact, it delivers a strict upper bound for the penetration depth
-    /// a predefined threshold, the results are further refined using the Expanding Polytope
+    /// a predefined threshold, the results are further refined using the Expanding Polytope 
     /// </summary>
     /// <param name="supportA">The support function of shape A.</param>
     /// <param name="supportB">The support function of shape B.</param>