MeshReduction for not-just-Mesh

BepuPhysics/Collidables/BigCompound.cs

@@ -305,6 +305,13 @@ public readonly unsafe void FindLocalOverlaps<TOverlaps>(Vector3 min, Vector3 ma
             Tree.Sweep(min, max, sweep, maximumT, pool, ref enumerator);
         }
 
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public readonly void FindLocalOverlaps<TEnumerator>(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator)
+            where TEnumerator : IBreakableForEach<int>
+        {
+            Tree.GetOverlaps(min, max, pool, ref enumerator);
+        }
+
         /// <summary>
         /// Computes the inertia of a compound. Does not recenter the child poses.
         /// </summary>
@@ -328,13 +335,13 @@ public readonly BodyInertia ComputeInertia(Span<float> childMasses, Shapes shape
             var bodyInertia = CompoundBuilder.ComputeInertia(Children, childMasses, shapes, out centerOfMass);
             //Recentering moves the children around, so the tree needs to be updated.
             //Scanning through and explicitly shifting the nodes is slightly more efficient than updating leaf bounds and refitting.
-            for (int i = 0; i < Tree.NodeCount; ++i)
-            {
-                ref var node = ref Tree.Nodes[i];
-                node.A.Min -= centerOfMass;
-                node.A.Max -= centerOfMass;
-                node.B.Min -= centerOfMass;
-                node.B.Max -= centerOfMass;
+            for (int i = 0; i < Tree.NodeCount; ++i)
+            {
+                ref var node = ref Tree.Nodes[i];
+                node.A.Min -= centerOfMass;
+                node.A.Max -= centerOfMass;
+                node.B.Min -= centerOfMass;
+                node.B.Max -= centerOfMass;
             }
             return bodyInertia;
         }

BepuPhysics/Collidables/Compound.cs

@@ -28,28 +28,28 @@ public struct CompoundChild
     /// <summary>
     /// Index of the shape within whatever shape collection holds the compound's child shape data.
     /// </summary>
-    public TypedIndex ShapeIndex;
-
-    /// <summary>
-    /// Creates a compound child.
-    /// </summary>
-    /// <param name="pose">Pose of the compound child in the local space of the parent shape.</param>
-    /// <param name="shapeIndex">Index of the shape used by the child.</param>
-    public CompoundChild(in RigidPose pose, TypedIndex shapeIndex)
-    {
-        LocalOrientation = pose.Orientation;
-        LocalPosition = pose.Position;
-        ShapeIndex = shapeIndex;
-    }
-
+    public TypedIndex ShapeIndex;
+
+    /// <summary>
+    /// Creates a compound child.
+    /// </summary>
+    /// <param name="pose">Pose of the compound child in the local space of the parent shape.</param>
+    /// <param name="shapeIndex">Index of the shape used by the child.</param>
+    public CompoundChild(in RigidPose pose, TypedIndex shapeIndex)
+    {
+        LocalOrientation = pose.Orientation;
+        LocalPosition = pose.Position;
+        ShapeIndex = shapeIndex;
+    }
+
     /// <summary>
     /// Returns a reference to the memory of the <see cref="CompoundChild"/> as a <see cref="RigidPose"/>.
     /// </summary>
     /// <returns>Reference to this compound child as a pose.</returns>
     [UnscopedRef]
-    public ref RigidPose AsPose()
-    {
-        return ref Unsafe.As<CompoundChild, RigidPose>(ref this);
+    public ref RigidPose AsPose()
+    {
+        return ref Unsafe.As<CompoundChild, RigidPose>(ref this);
     }
 }
 
@@ -336,6 +336,24 @@ public unsafe void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(ref Buffer<Ove
         }
     }
 
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public void FindLocalOverlaps<TEnumerator>(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator)
+        where TEnumerator : IBreakableForEach<int>
+    {
+        for (int i = 0; i < Children.Length; ++i)
+        {
+            ref var child = ref Children[i];
+            shapes[child.ShapeIndex.Type].ComputeBounds(child.ShapeIndex.Index, child.LocalOrientation, out _, out _, out var childMin, out var childMax);
+            childMin += child.LocalPosition;
+            childMax += child.LocalPosition;
+            if (BoundingBox.Intersects(childMin, childMax, min, max))
+            {
+                if (!enumerator.LoopBody(i))
+                    return;
+            }
+        }
+    }
+
     public unsafe void FindLocalOverlaps<TOverlaps>(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlapsPointer)
         where TOverlaps : ICollisionTaskSubpairOverlaps
     {

BepuPhysics/Collidables/Mesh.cs

@@ -371,6 +371,17 @@ public readonly unsafe void FindLocalOverlaps<TOverlaps>(Vector3 min, Vector3 ma
             Tree.Sweep(Vector3.Min(scaledMin, scaledMax), Vector3.Max(scaledMin, scaledMax), scaledSweep, maximumT, pool, ref enumerator);
         }
 
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public readonly void FindLocalOverlaps<TEnumerator>(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator)
+            where TEnumerator : IBreakableForEach<int>
+        {
+            //The tree is built from unscaled source triangles, so the query AABB has to be brought into unscaled space.
+            //Take a min/max to compensate for negative scales.
+            var scaledMin = min * inverseScale;
+            var scaledMax = max * inverseScale;
+            Tree.GetOverlaps(Vector3.Min(scaledMin, scaledMax), Vector3.Max(scaledMin, scaledMax), pool, ref enumerator);
+        }
+
         public struct MeshTriangleSource : ITriangleSource
         {
             Mesh mesh;

BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs

@@ -6,8 +6,8 @@
 namespace BepuPhysics.CollisionDetection.CollisionTasks
 {
     public unsafe struct CompoundMeshContinuations<TCompound, TMesh> : ICompoundPairContinuationHandler<CompoundMeshReduction>
-        where TCompound : ICompoundShape
-        where TMesh : IHomogeneousCompoundShape<Triangle, TriangleWide>
+        where TCompound : struct, ICompoundShape
+        where TMesh : struct, IHomogeneousCompoundShape<Triangle, TriangleWide>
     {
         public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.CompoundMeshReduction;
 
@@ -23,8 +23,9 @@ public ref CompoundMeshReduction CreateContinuation<TCallbacks>(
             collisionBatcher.Pool.Take(pairOverlaps.Length, out continuation.QueryBounds);
             continuation.RegionCount = pairOverlaps.Length;
             continuation.MeshOrientation = pair.OrientationB;
-            //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future.
-            continuation.Mesh = (Mesh*)pair.B;
+            continuation.Mesh = pair.B;
+            continuation.FindLocalOverlapsThunk = MeshReductionThunks<TMesh>.FindLocalOverlaps;
+            continuation.GetLocalChildThunk = MeshReductionThunks<TMesh>.GetLocalChild;
             //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.
             continuation.RequiresFlip = pair.FlipMask == 0;
 

BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs

@@ -21,6 +21,18 @@ void FindLocalOverlaps<TOverlaps, TSubpairOverlaps>(ref Buffer<OverlapQueryForPa
 
         void FindLocalOverlaps<TOverlaps>(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps)
             where TOverlaps : ICollisionTaskSubpairOverlaps;
+
+        /// <summary>
+        /// Finds the indices of all children whose local-space bounding boxes overlap the given local-space AABB.
+        /// </summary>
+        /// <typeparam name="TEnumerator">Type of the enumerator that receives child indices.</typeparam>
+        /// <param name="min">Minimum corner of the query AABB in the compound's local space.</param>
+        /// <param name="max">Maximum corner of the query AABB in the compound's local space.</param>
+        /// <param name="pool">Pool used for any temporary allocations during traversal.</param>
+        /// <param name="shapes">Shape collection used to look up child bounds for compounds with heterogeneous children. May be null for homogeneous compounds that don't require it.</param>
+        /// <param name="enumerator">Enumerator that receives the indices of overlapping children.</param>
+        void FindLocalOverlaps<TEnumerator>(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator)
+            where TEnumerator : IBreakableForEach<int>;
     }
     public interface IConvexCompoundOverlapFinder
     {

BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs

@@ -3,7 +3,7 @@
 
 namespace BepuPhysics.CollisionDetection.CollisionTasks
 {
-    public struct ConvexMeshContinuations<TMesh> : IConvexCompoundContinuationHandler<MeshReduction> where TMesh : IHomogeneousCompoundShape<Triangle, TriangleWide>
+    public struct ConvexMeshContinuations<TMesh> : IConvexCompoundContinuationHandler<MeshReduction> where TMesh : struct, IHomogeneousCompoundShape<Triangle, TriangleWide>
     {
         public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.MeshReduction;
 
@@ -20,8 +20,9 @@ public unsafe ref MeshReduction CreateContinuation<TCallbacks>(
             continuation.RequiresFlip = pair.FlipMask == 0;
             continuation.QueryBounds.Min = pairQuery.Min;
             continuation.QueryBounds.Max = pairQuery.Max;
-            //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future.
             continuation.Mesh = pairQuery.Container;
+            continuation.FindLocalOverlapsThunk = MeshReductionThunks<TMesh>.FindLocalOverlaps;
+            continuation.GetLocalChildThunk = MeshReductionThunks<TMesh>.GetLocalChild;
             return ref continuation;
         }
 

BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs

@@ -6,8 +6,8 @@
 namespace BepuPhysics.CollisionDetection.CollisionTasks
 {
     public unsafe struct MeshPairContinuations<TMeshA, TMeshB> : ICompoundPairContinuationHandler<CompoundMeshReduction>
-        where TMeshA : IHomogeneousCompoundShape<Triangle, TriangleWide>
-        where TMeshB : IHomogeneousCompoundShape<Triangle, TriangleWide>
+        where TMeshA : struct, IHomogeneousCompoundShape<Triangle, TriangleWide>
+        where TMeshB : struct, IHomogeneousCompoundShape<Triangle, TriangleWide>
     {
         public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.CompoundMeshReduction;
 
@@ -29,8 +29,9 @@ public ref CompoundMeshReduction CreateContinuation<TCallbacks>(
             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.
             continuation.RequiresFlip = pair.FlipMask == 0;
-            //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future.
-            continuation.Mesh = (Mesh*)pair.B;
+            continuation.Mesh = pair.B;
+            continuation.FindLocalOverlapsThunk = MeshReductionThunks<TMeshB>.FindLocalOverlaps;
+            continuation.GetLocalChildThunk = MeshReductionThunks<TMeshB>.GetLocalChild;
 
             //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.

BepuPhysics/CollisionDetection/CompoundMeshReduction.cs

@@ -21,7 +21,10 @@ public unsafe struct CompoundMeshReduction : ICollisionTestContinuation
         //This uses all of the nonconvex reduction's logic, so we just nest it.
         public NonconvexReduction Inner;
 
-        public Mesh* Mesh; //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future.
+        //Type-erased mesh pointer plus the per-TMesh thunks. See MeshReduction for the rationale.
+        public void* Mesh;
+        public delegate*<void*, Vector3, Vector3, BufferPool, Shapes, ref MeshReduction.ChildEnumerator, void> FindLocalOverlapsThunk;
+        public delegate*<void*, int, out Triangle, void> GetLocalChildThunk;
 
         public void Create(int childManifoldCount, BufferPool pool)
         {
@@ -54,7 +57,8 @@ public bool TryFlush<TCallbacks>(int pairId, ref CollisionBatcher<TCallbacks> ba
                     ref var region = ref ChildManifoldRegions[i];
                     if (region.Count > 0)
                     {
-                        MeshReduction.ReduceManifolds(ref Triangles, ref Inner.Children, region.Start, region.Count, RequiresFlip, QueryBounds[i], meshOrientation, meshInverseOrientation, Mesh, batcher.Pool);
+                        MeshReduction.ReduceManifolds(ref Triangles, ref Inner.Children, region.Start, region.Count, RequiresFlip, QueryBounds[i], meshOrientation, meshInverseOrientation,
+                            Mesh, FindLocalOverlapsThunk, GetLocalChildThunk, batcher.Shapes, batcher.Pool);
                     }
                 }