From 4e038d190b7e97df01e6a81c8f74551fe487a09d Mon Sep 17 00:00:00 2001
From: Jorrit Rouwe <jrouwe@gmail.com>
Date: Sat, 4 Nov 2023 12:47:16 +0100
Subject: [PATCH] When the closest set is 0xf, v_len_sq = 0 so we don't need to
 test for this (#742)

---
 Jolt/Geometry/GJKClosestPoint.h | 62 +++++++++++----------------------
 1 file changed, 20 insertions(+), 42 deletions(-)

diff --git a/Jolt/Geometry/GJKClosestPoint.h b/Jolt/Geometry/GJKClosestPoint.h
index 1dc9cb10b..802dff75b 100644
--- a/Jolt/Geometry/GJKClosestPoint.h
+++ b/Jolt/Geometry/GJKClosestPoint.h
@@ -598,7 +598,6 @@ class GJKClosestPoint : public NonCopyable
 				mY[i] = x - mP[i];
 
 			// Determine the new closest point from Y to origin
-			bool needs_restart = false;
 			uint32 set;						// Set of points that form the new simplex
 			if (!GetClosest<false>(v_len_sq, v, v_len_sq, set))
 			{
@@ -606,26 +605,6 @@ class GJKClosestPoint : public NonCopyable
 				Trace("Failed to converge");
 #endif
 
-				// We failed to converge, restart
-				needs_restart = true;
-			}
-			else if (set == 0xf)
-			{
-#ifdef JPH_GJK_DEBUG
-				Trace("Full simplex");
-#endif
-
-				// If there are 4 points, x is inside the tetrahedron and we've found a hit
-				// Double check if this is indeed the case
-				if (v_len_sq <= tolerance_sq)
-					break;
-
-				// We failed to converge, restart
-				needs_restart = true;
-			}
-
-			if (needs_restart)
-			{
 				// Only allow 1 restart, if we still can't get a closest point
 				// we're so close that we return this as a hit
 				if (!allow_restart)
@@ -642,6 +621,16 @@ class GJKClosestPoint : public NonCopyable
 				v_len_sq = FLT_MAX;
 				continue;
 			}
+			else if (set == 0xf)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Full simplex");
+#endif
+
+				// We're inside the tetrahedron, we have a hit (verify that length of v is 0)
+				JPH_ASSERT(v_len_sq == 0.0f);
+				break;
+			}
 
 			// Update the points P to form the new simplex
 			// Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration
@@ -807,7 +796,6 @@ class GJKClosestPoint : public NonCopyable
 				mY[i] = x - (mQ[i] - mP[i]);
 
 			// Determine the new closest point from Y to origin
-			bool needs_restart = false;
 			uint32 set;						// Set of points that form the new simplex
 			if (!GetClosest<false>(v_len_sq, v, v_len_sq, set))
 			{
@@ -815,26 +803,6 @@ class GJKClosestPoint : public NonCopyable
 				Trace("Failed to converge");
 #endif
 
-				// We failed to converge, restart
-				needs_restart = true;
-			}
-			else if (set == 0xf)
-			{
-#ifdef JPH_GJK_DEBUG
-				Trace("Full simplex");
-#endif
-
-				// If there are 4 points, x is inside the tetrahedron and we've found a hit
-				// Double check that A and B are indeed touching according to our tolerance
-				if (v_len_sq <= tolerance_sq)
-					break;
-
-				// We failed to converge, restart
-				needs_restart = true;
-			}
-
-			if (needs_restart)
-			{
 				// Only allow 1 restart, if we still can't get a closest point
 				// we're so close that we return this as a hit
 				if (!allow_restart)
@@ -852,6 +820,16 @@ class GJKClosestPoint : public NonCopyable
 				v_len_sq = FLT_MAX;
 				continue;
 			}
+			else if (set == 0xf)
+			{
+#ifdef JPH_GJK_DEBUG
+				Trace("Full simplex");
+#endif
+
+				// We're inside the tetrahedron, we have a hit (verify that length of v is 0)
+				JPH_ASSERT(v_len_sq == 0.0f);
+				break;
+			}
 
 			// Update the points P and Q to form the new simplex
 			// Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration