Improve termination condition

notgiven688 · Dec 30, 2023 · fbfd7e3 · fbfd7e3
1 parent 047920d
commit fbfd7e3
Showing 1 changed file with 8 additions and 4 deletions.
diff --git a/src/GJKEPA.cs b/src/GJKEPA.cs
@@ -324,12 +324,16 @@ public bool Solve(out JVector point1, out JVector point2, out JVector normal, ou
                     }
 
                     ctri = Triangles[closestIndex];
+
                     JVector searchDir = ctri.ClosestToOrigin;
+                    double searchDirSq = ctri.ClosestToOriginSq;
+
                     if (originEnclosed) searchDir.Negate();
 
                     if(ctri.ClosestToOriginSq < NumericEpsilon)
                     {
                         searchDir = ctri.Normal;
+                        searchDirSq = ctri.NormalSq;
                     }
 
                     vPointer++;
@@ -338,14 +342,14 @@ public bool Solve(out JVector point1, out JVector point2, out JVector normal, ou
                     // Termination condition
                     // Can we further "extend" the convex hull by adding the new vertex?
                     //
-                    // n = closest triangle normal
                     // v = Vertices[vPointer] (support point)
                     // c = Triangles[Head].ClosestToOrigin
+                    // s = searchDir
                     //
-                    // abs(dot(c - v, n)) / len(n) < e <=> [dot(c - v, n)]^2 = e*e*n^2
-                    double deltaDist = JVector.Dot(ctri.ClosestToOrigin - Vertices[vPointer], ctri.Normal);
+                    // abs(dot(c - v, s)) / len(s) < e <=> [dot(c - v, s)]^2 = e*e*s^2
+                    double deltaDist = JVector.Dot(ctri.ClosestToOrigin - Vertices[vPointer], searchDir);
 
-                    if(deltaDist * deltaDist < CollideEpsilon * CollideEpsilon * ctri.NormalSq)
+                    if (deltaDist * deltaDist <= CollideEpsilon * CollideEpsilon * searchDirSq)
                     {
                         goto converged;
                     }