Modify the case where ClosestToOriginSq is smaller than NumericEpsilon

src/GJKEPA.cs

@@ -1,23 +1,23 @@
 /* Copyright <2021> <Thorben Linneweber>
-* 
+*
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
-* 
+*
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
-* 
+*
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
-* 
+*
 */
 using System;
 
@@ -138,7 +138,7 @@ public bool CalcBarycentric(in Triangle tri, out JVector result, bool clamp = fa
 
                 // Calculate the barycentric coordinates of the origin (0,0,0) projected
                 // onto the plane of the triangle.
-                // 
+                //
                 // [W. Heidrich, Journal of Graphics, GPU, and Game Tools,Volume 10, Issue 3, 2005.]
 
                 JVector u, v, w, tmp;
@@ -302,11 +302,12 @@ public bool Solve(out JVector point1, out JVector point2, out JVector normal, ou
 
                 int iter = 0;
                 Triangle ctri; // closest Triangle
+                double deltaDist = 0;
 
                 while (++iter < MaxIter)
                 {
                     this.Statistics.Iterations = iter;
-                    
+
                     // search for the closest triangle and check if the origin is enclosed
                     int closestIndex = -1;
                     double currentMin = double.MaxValue;
@@ -322,14 +323,14 @@ public bool Solve(out JVector point1, out JVector point2, out JVector normal, ou
 
                         if(!Triangles[i].FacingOrigin) originEnclosed = false;
                     }
-                    
+
                     ctri = Triangles[closestIndex];
                     JVector searchDir = ctri.ClosestToOrigin;
                     if (originEnclosed) searchDir.Negate();
 
                     if(ctri.ClosestToOriginSq < NumericEpsilon)
                     {
-                        searchDir = ctri.Normal;
+                        goto converged;
                     }
 
                     vPointer++;
@@ -339,11 +340,11 @@ public bool Solve(out JVector point1, out JVector point2, out JVector normal, ou
                     //     c = Triangles[Head].ClosestToOrigin (closest point on the polytope)
                     //     v = Vertices[vPointer] (support point)
                     //     e = CollideEpsilon
-                    // The termination condition reads: 
+                    // The termination condition reads:
                     //     abs(dot(normalize(c), v - c)) < e
                     //     <=>  abs(dot(c, v - c))/len(c) < e <=> abs((dot(c, v) - dot(c,c)))/len(c) < e
                     //     <=>  (dot(c, v) - dot(c,c))^2 < e^2*c^2 <=> (dot(c, v) - c^2)^2 < e^2*c^2
-                    double deltaDist = ctri.ClosestToOriginSq - JVector.Dot(Vertices[vPointer], ctri.ClosestToOrigin);
+                    deltaDist = ctri.ClosestToOriginSq - JVector.Dot(Vertices[vPointer], ctri.ClosestToOrigin);
 
                     if(deltaDist * deltaDist < CollideEpsilon * CollideEpsilon * ctri.ClosestToOriginSq)
                     {