When the closest set is 0xf, v_len_sq = 0 so we don't need to test fo…

…r this (#742)
jrouwe · Nov 4, 2023 · 4e038d1 · 4e038d1
1 parent 912bdba
commit 4e038d1
Showing 1 changed file with 20 additions and 42 deletions.
diff --git a/Jolt/Geometry/GJKClosestPoint.h b/Jolt/Geometry/GJKClosestPoint.h
@@ -598,34 +598,13 @@ 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))
 			{
 #ifdef JPH_GJK_DEBUG
 				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,34 +796,13 @@ 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))
 			{
 #ifdef JPH_GJK_DEBUG
 				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