From fbfd7e386477283da9d4cd5f7c26eedaee21a5f7 Mon Sep 17 00:00:00 2001 From: notgiven688 Date: Sat, 30 Dec 2023 16:00:47 +0100 Subject: [PATCH] Improve termination condition --- src/GJKEPA.cs | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/GJKEPA.cs b/src/GJKEPA.cs index 18ddad1..551577f 100644 --- 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; }