Refactored continuations to provide query bounds data. Mesh reduction…

…s don't yet make use of the information.

BepuPhysics/Collidables/BigCompound.cs

@@ -160,7 +160,7 @@ public bool LoopBody(int i)
             }
         }
 
-        public unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(PairsToTestForOverlap* pairs, int count, BufferPool pool, Shapes shapes, ref TOverlaps overlaps)
+        public unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(ref Buffer<OverlapQueryForPair> pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps)
             where TOverlaps : struct, ICollisionTaskOverlaps<TSubpairOverlaps>
             where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps
         {
@@ -169,7 +169,7 @@ public unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(PairsToTestFor
             //using the fact that we have lots of independent queries to ensure the CPU always has something to do.
             Enumerator<TSubpairOverlaps> enumerator;
             enumerator.Pool = pool;
-            for (int i = 0; i < count; ++i)
+            for (int i = 0; i < pairs.Length; ++i)
             {
                 ref var pair = ref pairs[i];
                 enumerator.Overlaps = Unsafe.AsPointer(ref overlaps.GetOverlapsForPair(i));

BepuPhysics/Collidables/Compound.cs

@@ -149,11 +149,11 @@ public ref CompoundChild GetChild(int compoundChildIndex)
             return ref Children[compoundChildIndex];
         }
 
-        public unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(PairsToTestForOverlap* pairs, int count, BufferPool pool, Shapes shapes, ref TOverlaps overlaps)
+        public unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(ref Buffer<OverlapQueryForPair> pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps)
             where TOverlaps : struct, ICollisionTaskOverlaps<TSubpairOverlaps>
             where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps
         {
-            for (int pairIndex = 0; pairIndex < count; ++pairIndex)
+            for (int pairIndex = 0; pairIndex < pairs.Length; ++pairIndex)
             {
                 ref var pair = ref pairs[pairIndex];
                 ref var compound = ref Unsafe.AsRef<Compound>(pair.Container);

BepuPhysics/Collidables/Mesh.cs

@@ -279,7 +279,7 @@ public unsafe void RayTest<TRayHitHandler>(in RigidPose pose, ref RaySource rays
             }
         }
 
-        public unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(PairsToTestForOverlap* pairs, int count, BufferPool pool, Shapes shapes, ref TOverlaps overlaps)
+        public unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(ref Buffer<OverlapQueryForPair> pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps)
             where TOverlaps : struct, ICollisionTaskOverlaps<TSubpairOverlaps>
             where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps
         {
@@ -288,7 +288,7 @@ public unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(PairsToTestFor
             //using the fact that we have lots of independent queries to ensure the CPU always has something to do.
             ShapeTreeOverlapEnumerator<TSubpairOverlaps> enumerator;
             enumerator.Pool = pool;
-            for (int i = 0; i < count; ++i)
+            for (int i = 0; i < pairs.Length; ++i)
             {
                 ref var pair = ref pairs[i];
                 ref var mesh = ref Unsafe.AsRef<Mesh>(pair.Container);

BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs

@@ -1,5 +1,6 @@
 using BepuPhysics.Collidables;
 using BepuUtilities.Memory;
+using System.Diagnostics;
 using System.Runtime.CompilerServices;
 
 namespace BepuPhysics.CollisionDetection.CollisionTasks
@@ -12,13 +13,14 @@ public unsafe struct CompoundMeshContinuations<TCompound, TMesh> : ICompoundPair
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public ref CompoundMeshReduction CreateContinuation<TCallbacks>(
-            ref CollisionBatcher<TCallbacks> collisionBatcher, int totalChildCount, ref Buffer<ChildOverlapsCollection> pairOverlaps, in BoundsTestedPair pair, out int continuationIndex)
+            ref CollisionBatcher<TCallbacks> collisionBatcher, int totalChildCount, ref Buffer<ChildOverlapsCollection> pairOverlaps, ref Buffer<OverlapQueryForPair> pairQueries, in BoundsTestedPair pair, out int continuationIndex)
             where TCallbacks : struct, ICollisionCallbacks
         {
             ref var continuation = ref collisionBatcher.CompoundMeshReductions.CreateContinuation(totalChildCount, collisionBatcher.Pool, out continuationIndex);
             //Pass ownership of the triangle and region buffers to the continuation. It'll dispose of the buffer.
             collisionBatcher.Pool.Take(totalChildCount, out continuation.Triangles);
             collisionBatcher.Pool.Take(pairOverlaps.Length, out continuation.ChildManifoldRegions);
+            collisionBatcher.Pool.Take(pairOverlaps.Length, out continuation.QueryBounds);
             continuation.RegionCount = pairOverlaps.Length;
             continuation.MeshOrientation = pair.OrientationB;
             //A flip is required in mesh reduction whenever contacts are being generated as if the triangle is in slot B, which is whenever this pair has *not* been flipped.
@@ -27,11 +29,16 @@ public ref CompoundMeshReduction CreateContinuation<TCallbacks>(
             //All regions must be assigned ahead of time. Some trailing regions may be empty, so the dispatch may occur before all children are visited in the later loop.
             //That would result in potentially uninitialized values in region counts.
             int nextContinuationChildIndex = 0;
+            Debug.Assert(pairOverlaps.Length == pairQueries.Length);
             for (int j = 0; j < pairOverlaps.Length; ++j)
             {
                 ref var childOverlaps = ref pairOverlaps[j];
                 continuation.ChildManifoldRegions[j] = (nextContinuationChildIndex, childOverlaps.Count);
                 nextContinuationChildIndex += childOverlaps.Count;
+                ref var continuationBounds = ref continuation.QueryBounds[j];
+                ref var sourceBounds = ref pairQueries[j];
+                continuationBounds.Min = sourceBounds.Min;
+                continuationBounds.Max = sourceBounds.Max;
             }
             return ref continuation;
         }

BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairCollisionTask.cs

@@ -5,6 +5,7 @@
 using System;
 using System.Diagnostics;
 using System.Numerics;
+using System.Runtime.CompilerServices;
 using Quaternion = BepuUtilities.Quaternion;
 
 namespace BepuPhysics.CollisionDetection.CollisionTasks
@@ -18,7 +19,7 @@ public unsafe interface ICompoundPairContinuationHandler<TContinuation> where TC
     {
         CollisionContinuationType CollisionContinuationType { get; }
 
-        ref TContinuation CreateContinuation<TCallbacks>(ref CollisionBatcher<TCallbacks> collisionBatcher, int childCount, ref Buffer<ChildOverlapsCollection> pairOverlaps, in BoundsTestedPair pair, out int continuationIndex)
+        ref TContinuation CreateContinuation<TCallbacks>(ref CollisionBatcher<TCallbacks> collisionBatcher, int childCount, ref Buffer<ChildOverlapsCollection> pairOverlaps, ref Buffer<OverlapQueryForPair> pairQueries, in BoundsTestedPair pair, out int continuationIndex)
             where TCallbacks : struct, ICollisionCallbacks;
 
         void GetChildAData<TCallbacks>(ref CollisionBatcher<TCallbacks> collisionBatcher, ref TContinuation continuation, in BoundsTestedPair pair, int childIndexA, out RigidPose childPoseA, out int childTypeA, out void* childShapeDataA)
@@ -57,7 +58,7 @@ public unsafe override void ExecuteBatch<TCallbacks>(ref UntypedList batch, ref
             overlapFinder.FindLocalOverlaps(ref pairs, batch.Count, batcher.Pool, batcher.Shapes, batcher.Dt, out var overlaps);
             for (int pairIndex = 0; pairIndex < batch.Count; ++pairIndex)
             {
-                overlaps.GetPairOverlaps(pairIndex, out var pairOverlaps);
+                overlaps.GetPairOverlaps(pairIndex, out var pairOverlaps, out var subpairQueries);
                 var totalOverlapCountForPair = pairOverlaps[0].Count;
                 for (int j = 1; j < pairOverlaps.Length; ++j)
                 {
@@ -66,7 +67,7 @@ public unsafe override void ExecuteBatch<TCallbacks>(ref UntypedList batch, ref
                 if (totalOverlapCountForPair > 0)
                 {
                     ref var pair = ref pairs[pairIndex];
-                    ref var continuation = ref continuationHandler.CreateContinuation(ref batcher, totalOverlapCountForPair, ref pairOverlaps, pair, out var continuationIndex);
+                    ref var continuation = ref continuationHandler.CreateContinuation(ref batcher, totalOverlapCountForPair, ref pairOverlaps, ref subpairQueries, pair, out var continuationIndex);
 
                     var nextContinuationChildIndex = 0;
                     for (int j = 0; j < pairOverlaps.Length; ++j)

BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairContinuations.cs

@@ -12,7 +12,7 @@ public unsafe struct CompoundPairContinuations<TCompoundA, TCompoundB> : ICompou
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public ref NonconvexReduction CreateContinuation<TCallbacks>(
-            ref CollisionBatcher<TCallbacks> collisionBatcher, int totalChildCount, ref Buffer<ChildOverlapsCollection> pairOverlaps, in BoundsTestedPair pair, out int continuationIndex)
+            ref CollisionBatcher<TCallbacks> collisionBatcher, int totalChildCount, ref Buffer<ChildOverlapsCollection> pairOverlaps, ref Buffer<OverlapQueryForPair> pairQueries, in BoundsTestedPair pair, out int continuationIndex)
             where TCallbacks : struct, ICollisionCallbacks
         {
             return ref collisionBatcher.NonconvexReductions.CreateContinuation(totalChildCount, collisionBatcher.Pool, out continuationIndex);

BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlapFinder.cs

@@ -29,14 +29,14 @@ public unsafe void FindLocalOverlaps(ref Buffer<BoundsTestedPair> pairs, int pai
                 totalCompoundChildCount += Unsafe.AsRef<TCompoundA>(pairs[i].A).ChildCount;
             }
             overlaps = new CompoundPairOverlaps(pool, pairCount, totalCompoundChildCount);
-            var pairsToTest = stackalloc PairsToTestForOverlap[totalCompoundChildCount];
+            ref var pairsToTest = ref overlaps.pairQueries;
             var subpairData = stackalloc SubpairData[totalCompoundChildCount];
             int nextSubpairIndex = 0;
             for (int i = 0; i < pairCount; ++i)
             {
                 ref var pair = ref pairs[i];
                 ref var compoundA = ref Unsafe.AsRef<TCompoundA>(pair.A);
-                overlaps.CreatePairOverlaps(compoundA.ChildCount, pool);
+                overlaps.CreatePairOverlaps(compoundA.ChildCount);
                 for (int j = 0; j < compoundA.ChildCount; ++j)
                 {
                     var subpairIndex = nextSubpairIndex++;
@@ -117,7 +117,7 @@ out GatherScatter.Get(ref maximumRadius, j),
 
             //Doesn't matter what mesh/compound instance is used for the function; just using it as a source of the function.
             Debug.Assert(totalCompoundChildCount > 0);
-            Unsafe.AsRef<TCompoundB>(pairsToTest->Container).FindLocalOverlaps<CompoundPairOverlaps, ChildOverlapsCollection>(pairsToTest, totalCompoundChildCount, pool, shapes, ref overlaps);
+            Unsafe.AsRef<TCompoundB>(pairsToTest[0].Container).FindLocalOverlaps<CompoundPairOverlaps, ChildOverlapsCollection>(ref pairsToTest, pool, shapes, ref overlaps);
 
         }
 

BepuPhysics/CollisionDetection/CollisionTasks/CompoundPairOverlaps.cs

@@ -25,7 +25,7 @@ public unsafe struct ChildOverlapsCollection : ICollisionTaskSubpairOverlaps
         public Buffer<int> Overlaps;
         public int Count;
         public int ChildIndex;
-        
+
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public unsafe ref int Allocate(BufferPool pool)
         {
@@ -47,6 +47,7 @@ public void Dispose(BufferPool pool)
     public struct CompoundPairOverlaps : ICollisionTaskOverlaps<ChildOverlapsCollection>
     {
         Buffer<ChildOverlapsCollection> childOverlaps;
+        internal Buffer<OverlapQueryForPair> pairQueries;
         Buffer<(int start, int count)> pairRegions;
         int pairCount;
         int subpairCount;
@@ -55,13 +56,15 @@ public CompoundPairOverlaps(BufferPool pool, int pairCapacity, int subpairCapaci
             this.pairCount = 0;
             this.subpairCount = 0;
             pool.Take(subpairCapacity, out childOverlaps);
+            pool.Take(subpairCapacity, out pairQueries);
+            pairQueries.Slice(0, subpairCapacity, out pairQueries);
             //We rely on the length being zero to begin with for lazy initialization.
             childOverlaps.Clear(0, subpairCapacity);
             pool.Take(pairCapacity, out pairRegions);
         }
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void CreatePairOverlaps(int childrenInPair, BufferPool pool)
+        public void CreatePairOverlaps(int childrenInPair)
         {
             pairRegions[pairCount++] = (subpairCount, childrenInPair);
             subpairCount += childrenInPair;
@@ -75,10 +78,11 @@ public ref ChildOverlapsCollection GetOverlapsForPair(int subpairIndex)
             return ref childOverlaps[subpairIndex];
         }
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public void GetPairOverlaps(int pairIndex, out Buffer<ChildOverlapsCollection> pairOverlaps)
+        public void GetPairOverlaps(int pairIndex, out Buffer<ChildOverlapsCollection> pairOverlaps, out Buffer<OverlapQueryForPair> pairQueries)
         {
             ref var region = ref pairRegions[pairIndex];
             childOverlaps.Slice(region.start, region.count, out pairOverlaps);
+            this.pairQueries.Slice(region.start, region.count, out pairQueries);
         }
 
         public void Dispose(BufferPool pool)
@@ -87,6 +91,7 @@ public void Dispose(BufferPool pool)
             {
                 childOverlaps[i].Dispose(pool);
             }
+            pool.Return(ref pairQueries);
             pool.Return(ref childOverlaps);
             pool.Return(ref pairRegions);
         }

BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundCollisionTask.cs

@@ -14,7 +14,7 @@ public unsafe interface IConvexCompoundContinuationHandler<TContinuation> where
     {
         CollisionContinuationType CollisionContinuationType { get; }
 
-        ref TContinuation CreateContinuation<TCallbacks>(ref CollisionBatcher<TCallbacks> collisionBatcher, int childCount, in BoundsTestedPair pair, out int continuationIndex)
+        ref TContinuation CreateContinuation<TCallbacks>(ref CollisionBatcher<TCallbacks> collisionBatcher, int childCount, in BoundsTestedPair pair, in OverlapQueryForPair queryForPair, out int continuationIndex)
             where TCallbacks : struct, ICollisionCallbacks;
 
         void ConfigureContinuationChild<TCallbacks>(
@@ -50,11 +50,12 @@ public unsafe override void ExecuteBatch<TCallbacks>(ref UntypedList batch, ref
             for (int i = 0; i < batch.Count; ++i)
             {
                 ref var pairOverlaps = ref overlaps.GetOverlapsForPair(i);
+                ref var pairQuery = ref overlaps.GetQueryForPair(i);
                 if (pairOverlaps.Count > 0)
                 {
                     ref var pair = ref pairs[i];
                     ref var compound = ref Unsafe.AsRef<TCompound>(pair.B);
-                    ref var continuation = ref continuationHandler.CreateContinuation(ref batcher, pairOverlaps.Count, pair, out var continuationIndex);
+                    ref var continuation = ref continuationHandler.CreateContinuation(ref batcher, pairOverlaps.Count, pair, pairQuery, out var continuationIndex);
 
                     int nextContinuationChildIndex = 0;
                     for (int j = 0; j < pairOverlaps.Count; ++j)

BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundContinuations.cs

@@ -12,7 +12,7 @@ public struct ConvexCompoundContinuations<TCompound> : IConvexCompoundContinuati
 
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         public ref NonconvexReduction CreateContinuation<TCallbacks>(
-            ref CollisionBatcher<TCallbacks> collisionBatcher, int childCount, in BoundsTestedPair pair, out int continuationIndex)
+            ref CollisionBatcher<TCallbacks> collisionBatcher, int childCount, in BoundsTestedPair pair, in OverlapQueryForPair pairQuery, out int continuationIndex)
             where TCallbacks : struct, ICollisionCallbacks
         {
             return ref collisionBatcher.NonconvexReductions.CreateContinuation(childCount, collisionBatcher.Pool, out continuationIndex);

BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs

@@ -11,7 +11,7 @@
 
 namespace BepuPhysics.CollisionDetection.CollisionTasks
 {
-    public unsafe struct PairsToTestForOverlap
+    public unsafe struct OverlapQueryForPair
     {
         public void* Container;
         public Vector3 Min;
@@ -20,7 +20,7 @@ public unsafe struct PairsToTestForOverlap
 
     public unsafe interface IBoundsQueryableCompound
     {
-        unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(PairsToTestForOverlap* pairs, int count, BufferPool pool, Shapes shapes, ref TOverlaps overlaps)
+        unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(ref Buffer<OverlapQueryForPair> pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps)
             where TOverlaps : struct, ICollisionTaskOverlaps<TSubpairOverlaps>
             where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps;
 
@@ -41,8 +41,7 @@ public struct ConvexCompoundOverlapFinder<TConvex, TConvexWide, TCompound> : ICo
         public unsafe void FindLocalOverlaps(ref Buffer<BoundsTestedPair> pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps)
         {
             overlaps = new ConvexCompoundTaskOverlaps(pool, pairCount);
-            var pairsToTest = stackalloc PairsToTestForOverlap[pairCount];
-
+            ref var pairsToTest = ref overlaps.subpairQueries;
             Vector3Wide offsetB = default;
             QuaternionWide orientationA = default;
             QuaternionWide orientationB = default;
@@ -100,7 +99,7 @@ public unsafe void FindLocalOverlaps(ref Buffer<BoundsTestedPair> pairs, int pai
             }
 
             //The choice of instance here is irrelevant.
-            Unsafe.AsRef<TCompound>(pairsToTest->Container).FindLocalOverlaps<ConvexCompoundTaskOverlaps, ConvexCompoundOverlaps>(pairsToTest, pairCount, pool, shapes, ref overlaps);
+            Unsafe.AsRef<TCompound>(pairsToTest[0].Container).FindLocalOverlaps<ConvexCompoundTaskOverlaps, ConvexCompoundOverlaps>(ref pairsToTest, pool, shapes, ref overlaps);
 
         }
 

BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundTaskOverlaps.cs

@@ -30,10 +30,13 @@ public void Dispose(BufferPool pool)
     public struct ConvexCompoundTaskOverlaps : ICollisionTaskOverlaps<ConvexCompoundOverlaps>
     {
         Buffer<ConvexCompoundOverlaps> overlaps;
+        internal Buffer<OverlapQueryForPair> subpairQueries;
         public ConvexCompoundTaskOverlaps(BufferPool pool, int pairCount)
         {
             pool.Take(pairCount, out overlaps);
+            pool.Take(pairCount, out subpairQueries);
             overlaps.Slice(0, pairCount, out overlaps);
+            subpairQueries.Slice(0, pairCount, out subpairQueries);
             //We rely on the length being zero to begin with for lazy initialization.
             overlaps.Clear(0, pairCount);
         }
@@ -43,13 +46,19 @@ public ref ConvexCompoundOverlaps GetOverlapsForPair(int pairIndex)
         {
             return ref overlaps[pairIndex];
         }
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public ref OverlapQueryForPair GetQueryForPair(int pairIndex)
+        {
+            return ref subpairQueries[pairIndex];
+        }
 
         public void Dispose(BufferPool pool)
         {
             for (int i = 0; i < overlaps.Length; ++i)
             {
                 overlaps[i].Dispose(pool);
             }
+            pool.Return(ref subpairQueries);
             pool.Return(ref overlaps);
         }