From e480b2af4c17f5201547e5eadf4b78b3eb76b873 Mon Sep 17 00:00:00 2001
From: Ross Nordby <norbo@bepuentertainment.com>
Date: Sat, 16 Nov 2024 18:58:28 -0600
Subject: [PATCH] Debug improvements and limited edge filtering.

---
 BepuPhysics/Collidables/ConvexHullHelper.cs  | 34 ++++++++++++--------
 Demos/SpecializedTests/ConvexHullTestDemo.cs |  8 ++---
 2 files changed, 25 insertions(+), 17 deletions(-)

diff --git a/BepuPhysics/Collidables/ConvexHullHelper.cs b/BepuPhysics/Collidables/ConvexHullHelper.cs
index 4560f188..e5f8267d 100644
--- a/BepuPhysics/Collidables/ConvexHullHelper.cs
+++ b/BepuPhysics/Collidables/ConvexHullHelper.cs
@@ -1,4 +1,5 @@
-using BepuUtilities;
+using BepuPhysics.Constraints.Contact;
+using BepuUtilities;
 using BepuUtilities.Collections;
 using BepuUtilities.Memory;
 using System;
@@ -506,12 +507,15 @@ static void AddFaceEdgesToTestList(BufferPool pool,
                 endpoints.A = previousIndex;
                 endpoints.B = reducedFaceIndices[i];
                 previousIndex = endpoints.B;
-                EdgeToTest nextEdgeToTest;
-                nextEdgeToTest.Endpoints = endpoints;
-                nextEdgeToTest.FaceNormal = faceNormal;
-                nextEdgeToTest.FaceIndex = newFaceIndex;
-                edgesToTest.Allocate(pool) = nextEdgeToTest;
-                submittedEdgeTests.Add(endpoints, pool);
+                if (!submittedEdgeTests.Contains(endpoints))
+                {
+                    EdgeToTest nextEdgeToTest;
+                    nextEdgeToTest.Endpoints = endpoints;
+                    nextEdgeToTest.FaceNormal = faceNormal;
+                    nextEdgeToTest.FaceIndex = newFaceIndex;
+                    edgesToTest.Allocate(pool) = nextEdgeToTest;
+                    submittedEdgeTests.Add(endpoints, pool);
+                }
             }
         }
 
@@ -526,8 +530,7 @@ public struct DebugStep
             public EdgeEndpoints SourceEdge;
             public int[] Raw;
             public int[] Reduced;
-            public int[] RawOverwrittenByMerge;
-            public int[] ReducedOverwrittenByMerge;
+            public int[] OverwrittenOriginal;
             public bool[] AllowVertex;
             public Vector3 FaceNormal;
             public Vector3 BasisX;
@@ -546,8 +549,7 @@ internal DebugStep(EdgeEndpoints sourceEdge, QuickList<int> rawVertexIndices, Ve
                 BasisY = basisY;
                 Raw = ((Span<int>)rawVertexIndices).ToArray();
                 Reduced = ((Span<int>)reducedVertexIndices).ToArray();
-                RawOverwrittenByMerge = null;
-                ReducedOverwrittenByMerge = null;      
+                OverwrittenOriginal = null;    
                 FaceIndex = faceIndex;
             }
 
@@ -572,10 +574,13 @@ internal DebugStep FillHistory(Buffer<int> allowVertex, QuickList<EarlyFace> fac
                 return this;
             }
 
+            internal void RecordDeletedFace(QuickList<int> faceVertexIndices)
+            {
+                OverwrittenOriginal = ((Span<int>)faceVertexIndices).ToArray();
+            }
+
             internal void UpdateForFaceMerge(QuickList<int> rawFaceVertexIndices, QuickList<int> reducedVertexIndices, Buffer<int> allowVertex, int mergedFaceIndex)
             {
-                RawOverwrittenByMerge = Raw;
-                ReducedOverwrittenByMerge = Reduced;
                 Raw = ((Span<int>)rawFaceVertexIndices).ToArray();
                 Reduced = ((Span<int>)reducedVertexIndices).ToArray();
                 FaceIndex = mergedFaceIndex;
@@ -815,12 +820,15 @@ public static void ComputeHull(Span<Vector3> points, BufferPool pool, out HullDa
                             }
                         }
                         // Rerun reduction for the merged face.
+                        step.RecordDeletedFace(face.VertexIndices);
                         face.VertexIndices.Count = 0;
                         facePoints.Count = 0;
                         face.VertexIndices.EnsureCapacity(rawFaceVertexIndices.Count, pool);
                         ReduceFace(ref rawFaceVertexIndices, faceNormal, points, planeEpsilonNarrow, ref facePoints, ref allowVertices, ref face.VertexIndices);
                         step.UpdateForFaceMerge(rawFaceVertexIndices, face.VertexIndices, allowVertices, i);
                         mergedFace = true;
+                        
+                        // It's possible for the merged face to have invalidated a previous face that wouldn't necessarily be detected as something to merge.
                         break;
                     }
                 }
diff --git a/Demos/SpecializedTests/ConvexHullTestDemo.cs b/Demos/SpecializedTests/ConvexHullTestDemo.cs
index 4ce7a485..3e7d2572 100644
--- a/Demos/SpecializedTests/ConvexHullTestDemo.cs
+++ b/Demos/SpecializedTests/ConvexHullTestDemo.cs
@@ -756,7 +756,7 @@ void DrawVertexIndex(int i, Vector3 color, Vector2 offset = default)
 
         {
             var pose = new RigidPose(renderOffset);
-            void DrawFace(DebugStep step, int[] raw, int[] reduced, bool deleted, int i)
+            void DrawFace(DebugStep step, int[] reduced, bool deleted, int i)
             {
                 var color = deleted ? new Vector3(0.25f, 0.25f, 0.25f) : stepIndex == i ? new Vector3(1, 0, 0.5f) : new Vector3(1, 0, 1);
                 var deletionInducedScale = deleted ? new Vector3(1.1f) : new Vector3(1f);
@@ -789,10 +789,10 @@ void DrawFace(DebugStep step, int[] raw, int[] reduced, bool deleted, int i)
             for (int i = 0; i <= stepIndex; ++i)
             {
                 var localStep = debugSteps[i];
-                DrawFace(localStep, localStep.Raw, localStep.Reduced, false, i);
-                if (localStep.RawOverwrittenByMerge != null)
+                DrawFace(localStep, localStep.Reduced, false, i);
+                if (localStep.OverwrittenOriginal != null)
                 {
-                    DrawFace(localStep, localStep.RawOverwrittenByMerge, localStep.ReducedOverwrittenByMerge, true, i);
+                    DrawFace(localStep, localStep.OverwrittenOriginal, true, i);
                 }
 
             }