diff --git a/BepuPhysics/Collidables/BigCompound.cs b/BepuPhysics/Collidables/BigCompound.cs index 644e2c1d..33d044bc 100644 --- a/BepuPhysics/Collidables/BigCompound.cs +++ b/BepuPhysics/Collidables/BigCompound.cs @@ -305,6 +305,13 @@ public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 ma Tree.Sweep(min, max, sweep, maximumT, pool, ref enumerator); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public readonly void FindLocalOverlaps(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator) + where TEnumerator : IBreakableForEach + { + Tree.GetOverlaps(min, max, pool, ref enumerator); + } + /// /// Computes the inertia of a compound. Does not recenter the child poses. /// @@ -328,13 +335,13 @@ public readonly BodyInertia ComputeInertia(Span 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; } diff --git a/BepuPhysics/Collidables/Compound.cs b/BepuPhysics/Collidables/Compound.cs index 9a1da6d5..dc42ec0c 100644 --- a/BepuPhysics/Collidables/Compound.cs +++ b/BepuPhysics/Collidables/Compound.cs @@ -28,28 +28,28 @@ public struct CompoundChild /// /// Index of the shape within whatever shape collection holds the compound's child shape data. /// - public TypedIndex ShapeIndex; - - /// - /// Creates a compound child. - /// - /// Pose of the compound child in the local space of the parent shape. - /// Index of the shape used by the child. - public CompoundChild(in RigidPose pose, TypedIndex shapeIndex) - { - LocalOrientation = pose.Orientation; - LocalPosition = pose.Position; - ShapeIndex = shapeIndex; - } - + public TypedIndex ShapeIndex; + + /// + /// Creates a compound child. + /// + /// Pose of the compound child in the local space of the parent shape. + /// Index of the shape used by the child. + public CompoundChild(in RigidPose pose, TypedIndex shapeIndex) + { + LocalOrientation = pose.Orientation; + LocalPosition = pose.Position; + ShapeIndex = shapeIndex; + } + /// /// Returns a reference to the memory of the as a . /// /// Reference to this compound child as a pose. [UnscopedRef] - public ref RigidPose AsPose() - { - return ref Unsafe.As(ref this); + public ref RigidPose AsPose() + { + return ref Unsafe.As(ref this); } } @@ -336,6 +336,24 @@ public unsafe void FindLocalOverlaps(ref Buffer(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator) + where TEnumerator : IBreakableForEach + { + 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(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlapsPointer) where TOverlaps : ICollisionTaskSubpairOverlaps { diff --git a/BepuPhysics/Collidables/Mesh.cs b/BepuPhysics/Collidables/Mesh.cs index 68e40672..d5d078e1 100644 --- a/BepuPhysics/Collidables/Mesh.cs +++ b/BepuPhysics/Collidables/Mesh.cs @@ -371,6 +371,17 @@ public readonly unsafe void FindLocalOverlaps(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(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator) + where TEnumerator : IBreakableForEach + { + //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; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs index afcd9b98..7f8ee841 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/CompoundMeshContinuations.cs @@ -6,8 +6,8 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public unsafe struct CompoundMeshContinuations : ICompoundPairContinuationHandler - where TCompound : ICompoundShape - where TMesh : IHomogeneousCompoundShape + where TCompound : struct, ICompoundShape + where TMesh : struct, IHomogeneousCompoundShape { public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.CompoundMeshReduction; @@ -23,8 +23,9 @@ public ref CompoundMeshReduction CreateContinuation( 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.FindLocalOverlaps; + continuation.GetLocalChildThunk = MeshReductionThunks.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; diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs index a4116c88..6c226121 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexCompoundOverlapFinder.cs @@ -21,6 +21,18 @@ void FindLocalOverlaps(ref Buffer(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps; + + /// + /// Finds the indices of all children whose local-space bounding boxes overlap the given local-space AABB. + /// + /// Type of the enumerator that receives child indices. + /// Minimum corner of the query AABB in the compound's local space. + /// Maximum corner of the query AABB in the compound's local space. + /// Pool used for any temporary allocations during traversal. + /// Shape collection used to look up child bounds for compounds with heterogeneous children. May be null for homogeneous compounds that don't require it. + /// Enumerator that receives the indices of overlapping children. + void FindLocalOverlaps(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator) + where TEnumerator : IBreakableForEach; } public interface IConvexCompoundOverlapFinder { diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs index a4aeffeb..7428c120 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/ConvexMeshContinuations.cs @@ -3,7 +3,7 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { - public struct ConvexMeshContinuations : IConvexCompoundContinuationHandler where TMesh : IHomogeneousCompoundShape + public struct ConvexMeshContinuations : IConvexCompoundContinuationHandler where TMesh : struct, IHomogeneousCompoundShape { public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.MeshReduction; @@ -20,8 +20,9 @@ public unsafe ref MeshReduction CreateContinuation( 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.FindLocalOverlaps; + continuation.GetLocalChildThunk = MeshReductionThunks.GetLocalChild; return ref continuation; } diff --git a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs index 7afce481..0497ac87 100644 --- a/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs +++ b/BepuPhysics/CollisionDetection/CollisionTasks/MeshPairContinuations.cs @@ -6,8 +6,8 @@ namespace BepuPhysics.CollisionDetection.CollisionTasks { public unsafe struct MeshPairContinuations : ICompoundPairContinuationHandler - where TMeshA : IHomogeneousCompoundShape - where TMeshB : IHomogeneousCompoundShape + where TMeshA : struct, IHomogeneousCompoundShape + where TMeshB : struct, IHomogeneousCompoundShape { public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.CompoundMeshReduction; @@ -29,8 +29,9 @@ public ref CompoundMeshReduction CreateContinuation( 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.FindLocalOverlaps; + continuation.GetLocalChildThunk = MeshReductionThunks.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. diff --git a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs b/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs index 23ba9c3c..5501b440 100644 --- a/BepuPhysics/CollisionDetection/CompoundMeshReduction.cs +++ b/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* FindLocalOverlapsThunk; + public delegate* GetLocalChildThunk; public void Create(int childManifoldCount, BufferPool pool) { @@ -54,7 +57,8 @@ public bool TryFlush(int pairId, ref CollisionBatcher 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); } } diff --git a/BepuPhysics/CollisionDetection/MeshReduction.cs b/BepuPhysics/CollisionDetection/MeshReduction.cs index 01ffe05e..0d08b0e6 100644 --- a/BepuPhysics/CollisionDetection/MeshReduction.cs +++ b/BepuPhysics/CollisionDetection/MeshReduction.cs @@ -29,7 +29,14 @@ public unsafe struct MeshReduction : ICollisionTestContinuation //This uses all of the nonconvex reduction's logic, so we just nest it. public NonconvexReduction Inner; - public void* 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 pointer to the mesh shape data, plus two function pointers that close over the concrete mesh type. + //ConvexMeshContinuations populates these from MeshReductionThunks at continuation creation time. + //The thunks are static methods of a generic helper, so they're JIT-specialized per TMesh and any interface + //call inside them is devirtualized. This keeps the 'way too many subpairs' path's per-contact calls cheap without requiring + //CollisionBatcher to know about TMesh. + public void* Mesh; + public delegate* FindLocalOverlapsThunk; + public delegate* GetLocalChildThunk; public void Create(int childManifoldCount, BufferPool pool) { @@ -280,7 +287,7 @@ static void TryApplyBlockToTriangle(ref TestTriangle triangle, Buffer + public struct ChildEnumerator : IBreakableForEach { public QuickList List; public BufferPool Pool; @@ -292,7 +299,11 @@ public bool LoopBody(int i) } public static void ReduceManifolds(ref Buffer continuationTriangles, ref Buffer continuationChildren, int start, int count, - bool requiresFlip, in BoundingBox queryBounds, in Matrix3x3 meshOrientation, in Matrix3x3 meshInverseOrientation, Mesh* mesh, BufferPool pool) + bool requiresFlip, in BoundingBox queryBounds, in Matrix3x3 meshOrientation, in Matrix3x3 meshInverseOrientation, + void* mesh, + delegate* findLocalOverlapsThunk, + delegate* getLocalChildThunk, + Shapes shapes, BufferPool pool) { //Before handing responsibility off to the nonconvex reduction, make sure that no contacts create nasty 'bumps' at the border of triangles. //Bumps can occur when an isolated triangle test detects a contact pointing outward, like when a box hits the side. This is fine when the triangle truly is isolated, @@ -445,7 +456,9 @@ public static void ReduceManifolds(ref Buffer continuationTriangles, r var contactQueryMin = meshSpaceContact - contactExpansion; var contactQueryMax = meshSpaceContact + contactExpansion; enumerator.List.Count = 0; - mesh->Tree.GetOverlaps(contactQueryMin, contactQueryMax, pool, ref enumerator); + //The thunk takes coordinates in the same space as the cached TestTriangle data (i.e. the space GetLocalChild returns), + //and is responsible for any internal coordinate-space conversion (e.g. Mesh applies its inverse scale before traversing the tree). + findLocalOverlapsThunk(mesh, contactQueryMin, contactQueryMax, pool, shapes, ref enumerator); //Note that the test triangles detected by querying may exceed the count in extremely rare cases, so it's not safe to use AllocateUnsafely without some extra work. //Resizing invalidates table indices, so do any that ahead of time. testTriangles.EnsureCapacity(testTriangles.Count + enumerator.List.Count, pool); @@ -459,7 +472,7 @@ public static void ReduceManifolds(ref Buffer continuationTriangles, r //1) in the long term, the mesh type will be abstracted away, and we might be dealing with a type that doesn't have a Triangles buffer at all. //2) the Mesh applies a scale to the stored triangles! That's why we have the continuation triangles explicitly stored rather than just looking them all up in the mesh- //the convex-triangle tests that preceded this reduction had to have somewhere they could load the 'baked' triangle data from. - mesh->GetLocalChild(triangleIndexInMesh, out var triangle); + getLocalChildThunk(mesh, triangleIndexInMesh, out var triangle); testTriangles.Values[triangleIndex] = new TestTriangle(triangle, triangleIndex); } ref var targetTriangle = ref testTriangles.Values[triangleIndex]; @@ -514,8 +527,8 @@ public bool TryFlush(int pairId, ref CollisionBatcher ba { Matrix3x3.CreateFromQuaternion(MeshOrientation, out var meshOrientation); Matrix3x3.Transpose(meshOrientation, out var meshInverseOrientation); - //TODO: This is not flexible with respect to different mesh types. Not a problem right now, but it will be in the future. - ReduceManifolds(ref Triangles, ref Inner.Children, 0, Inner.ChildCount, RequiresFlip, QueryBounds, meshOrientation, meshInverseOrientation, (Mesh*)Mesh, batcher.Pool); + ReduceManifolds(ref Triangles, ref Inner.Children, 0, Inner.ChildCount, RequiresFlip, QueryBounds, meshOrientation, meshInverseOrientation, + Mesh, FindLocalOverlapsThunk, GetLocalChildThunk, batcher.Shapes, batcher.Pool); //Now that boundary smoothing analysis is done, we no longer need the triangle list. batcher.Pool.Return(ref Triangles); @@ -526,4 +539,28 @@ public bool TryFlush(int pairId, ref CollisionBatcher ba } } + + /// + /// Type-specialized thunks that bridge MeshReduction's type-erased function pointer fields back to a concrete mesh shape type. + /// The static fields here are populated once per closed TMesh by the runtime, and the JIT specializes the bodies so that + /// the calls into are devirtualized. + /// + /// Concrete homogeneous triangle compound shape type. + public static unsafe class MeshReductionThunks where TMesh : struct, IHomogeneousCompoundShape + { + public static readonly delegate* FindLocalOverlaps = &FindLocalOverlapsImpl; + public static readonly delegate* GetLocalChild = &GetLocalChildImpl; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void FindLocalOverlapsImpl(void* mesh, Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref MeshReduction.ChildEnumerator enumerator) + { + Unsafe.AsRef(mesh).FindLocalOverlaps(min, max, pool, shapes, ref enumerator); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static void GetLocalChildImpl(void* mesh, int childIndex, out Triangle triangle) + { + Unsafe.AsRef(mesh).GetLocalChild(childIndex, out triangle); + } + } } diff --git a/Demos/Demos/CustomVoxelCollidableDemo.cs b/Demos/Demos/CustomVoxelCollidableDemo.cs index 060029a7..8f622907 100644 --- a/Demos/Demos/CustomVoxelCollidableDemo.cs +++ b/Demos/Demos/CustomVoxelCollidableDemo.cs @@ -108,7 +108,6 @@ unsafe struct HitLeafTester : IRayLeafTester where T : IShapeRayHitHandler public Matrix3x3 Orientation; public RayData OriginalRay; - [MethodImpl(MethodImplOptions.AggressiveInlining)] public void TestLeaf(int leafIndex, RayData* ray, float* maximumT, BufferPool pool) { ref var voxelIndex = ref VoxelIndices[leafIndex]; @@ -179,7 +178,6 @@ public readonly unsafe void RayTest(in RigidPose pose, ref RaySo hitHandler = leafTester.HitHandler; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public readonly void GetLocalChild(int childIndex, out Box childShape) { var halfSize = VoxelSize * 0.5f; @@ -188,14 +186,12 @@ public readonly void GetLocalChild(int childIndex, out Box childShape) childShape.HalfLength = halfSize.Z; } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public readonly void GetPosedLocalChild(int childIndex, out Box childShape, out RigidPose childPose) { GetLocalChild(childIndex, out childShape); childPose = (VoxelIndices[childIndex] + new Vector3(0.5f) * VoxelSize); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public readonly void GetLocalChild(int childIndex, ref BoxWide shapeWide) { //This function provides a reference to a lane in an AOSOA structure. @@ -207,7 +203,6 @@ public readonly void GetLocalChild(int childIndex, ref BoxWide shapeWide) } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public readonly unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) where TOverlaps : struct, ICollisionTaskOverlaps where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps @@ -228,6 +223,12 @@ public readonly unsafe void FindLocalOverlaps(ref B } } + public readonly void FindLocalOverlaps(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator) + where TEnumerator : IBreakableForEach + { + Tree.GetOverlaps(min, max, pool, ref enumerator); + } + public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) where TOverlaps : ICollisionTaskSubpairOverlaps { //Similar to the non-swept FindLocalOverlaps function above, this just adds the overlaps to the provided collection. @@ -257,7 +258,6 @@ public struct ConvexVoxelsContinuations : IConvexCompoundContinuationHandler CollisionContinuationType.NonconvexReduction; - [MethodImpl(MethodImplOptions.AggressiveInlining)] public ref NonconvexReduction CreateContinuation( ref CollisionBatcher collisionBatcher, int childCount, in BoundsTestedPair pair, in OverlapQueryForPair pairQuery, out int continuationIndex) where TCallbacks : struct, ICollisionCallbacks @@ -265,7 +265,6 @@ public ref NonconvexReduction CreateContinuation( return ref collisionBatcher.NonconvexReductions.CreateContinuation(childCount, collisionBatcher.Pool, out continuationIndex); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public static unsafe void GetChildData(ref CollisionBatcher collisionBatcher, ref NonconvexReductionChild continuationChild, in BoundsTestedPair pair, int shapeTypeA, int childIndexB, out RigidPose childPoseB, out int childTypeB, out void* childShapeDataB) where TCallbacks : struct, ICollisionCallbacks @@ -288,7 +287,6 @@ public static unsafe void GetChildData(ref CollisionBatcher( ref CollisionBatcher collisionBatcher, ref NonconvexReduction continuation, int continuationChildIndex, in BoundsTestedPair pair, int shapeTypeA, int childIndexB, out RigidPose childPoseB, out int childTypeB, out void* childShapeDataB) @@ -320,7 +318,6 @@ public unsafe struct CompoundVoxelsContinuations : ICompoundPairCont { public CollisionContinuationType CollisionContinuationType => CollisionContinuationType.NonconvexReduction; - [MethodImpl(MethodImplOptions.AggressiveInlining)] public ref NonconvexReduction CreateContinuation( ref CollisionBatcher collisionBatcher, int totalChildCount, ref Buffer pairOverlaps, ref Buffer pairQueries, in BoundsTestedPair pair, out int continuationIndex) where TCallbacks : struct, ICollisionCallbacks @@ -328,7 +325,6 @@ public ref NonconvexReduction CreateContinuation( return ref collisionBatcher.NonconvexReductions.CreateContinuation(totalChildCount, collisionBatcher.Pool, out continuationIndex); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public void GetChildAData(ref CollisionBatcher collisionBatcher, ref NonconvexReduction continuation, in BoundsTestedPair pair, int childIndexA, out RigidPose childPoseA, out int childTypeA, out void* childShapeDataA) where TCallbacks : struct, ICollisionCallbacks @@ -340,7 +336,6 @@ public void GetChildAData(ref CollisionBatcher collision collisionBatcher.Shapes[childTypeA].GetShapeData(compoundChildA.ShapeIndex.Index, out childShapeDataA, out _); } - [MethodImpl(MethodImplOptions.AggressiveInlining)] public void ConfigureContinuationChild( ref CollisionBatcher collisionBatcher, ref NonconvexReduction continuation, int continuationChildIndex, in BoundsTestedPair pair, int childIndexA, int childTypeA, int childIndexB, in RigidPose childPoseA, out RigidPose childPoseB, out int childTypeB, out void* childShapeDataB) diff --git a/Demos/SpecializedTests/CustomMeshSmoothingTestDemo.cs b/Demos/SpecializedTests/CustomMeshSmoothingTestDemo.cs new file mode 100644 index 00000000..252922a8 --- /dev/null +++ b/Demos/SpecializedTests/CustomMeshSmoothingTestDemo.cs @@ -0,0 +1,280 @@ +using System; +using System.Numerics; +using System.Runtime.CompilerServices; +using BepuPhysics; +using BepuPhysics.Collidables; +using BepuPhysics.CollisionDetection; +using BepuPhysics.CollisionDetection.CollisionTasks; +using BepuPhysics.CollisionDetection.SweepTasks; +using BepuPhysics.Constraints; +using BepuPhysics.Trees; +using BepuUtilities; +using BepuUtilities.Collections; +using BepuUtilities.Memory; +using DemoContentLoader; +using DemoRenderer; +using DemoRenderer.UI; +using DemoUtilities; + +namespace Demos.SpecializedTests; + +/// +/// Pure forwarding wrapper around . Has its own TypeId so the narrow phase treats it as a distinct shape, +/// which lets us verify that 's boundary smoothing works for any , not just the built-in Mesh type. +/// +public struct WrappedMesh : IHomogeneousCompoundShape +{ + public Mesh Inner; + + public WrappedMesh(Mesh inner) + { + Inner = inner; + } + + public const int Id = 13; + public static int TypeId => Id; + + public readonly int ChildCount => Inner.ChildCount; + + public static ShapeBatch CreateShapeBatch(BufferPool pool, int initialCapacity, Shapes shapeBatches) + { + return new HomogeneousCompoundShapeBatch(pool, initialCapacity); + } + + public readonly void ComputeBounds(Quaternion orientation, out Vector3 min, out Vector3 max) + { + Inner.ComputeBounds(orientation, out min, out max); + } + + public readonly void GetLocalChild(int childIndex, out Triangle target) + { + Inner.GetLocalChild(childIndex, out target); + } + + public readonly void GetPosedLocalChild(int childIndex, out Triangle target, out RigidPose childPose) + { + Inner.GetPosedLocalChild(childIndex, out target, out childPose); + } + + public readonly void GetLocalChild(int childIndex, ref TriangleWide target) + { + Inner.GetLocalChild(childIndex, ref target); + } + + public readonly void RayTest(in RigidPose pose, in RayData ray, ref float maximumT, BufferPool pool, ref TRayHitHandler hitHandler) + where TRayHitHandler : struct, IShapeRayHitHandler + { + Inner.RayTest(pose, ray, ref maximumT, pool, ref hitHandler); + } + + public readonly void RayTest(in RigidPose pose, ref RaySource rays, BufferPool pool, ref TRayHitHandler hitHandler) + where TRayHitHandler : struct, IShapeRayHitHandler + { + Inner.RayTest(pose, ref rays, pool, ref hitHandler); + } + + public readonly unsafe void FindLocalOverlaps(ref Buffer pairs, BufferPool pool, Shapes shapes, ref TOverlaps overlaps) + where TOverlaps : struct, ICollisionTaskOverlaps + where TSubpairOverlaps : struct, ICollisionTaskSubpairOverlaps + { + //Can't forward directly: the Mesh implementation reinterprets each pair.Container as Mesh*, but here the containers point to WrappedMesh instances. + //Replicate the loop and forward each pair's AABB to the inner mesh's single-AABB overload instead. + ShapeTreeOverlapEnumerator enumerator; + enumerator.Pool = pool; + for (int i = 0; i < pairs.Length; ++i) + { + ref var pair = ref pairs[i]; + ref var wrapped = ref Unsafe.AsRef(pair.Container); + enumerator.Overlaps = Unsafe.AsPointer(ref overlaps.GetOverlapsForPair(i)); + wrapped.Inner.FindLocalOverlaps(pair.Min, pair.Max, pool, shapes, ref enumerator); + } + } + + public readonly unsafe void FindLocalOverlaps(Vector3 min, Vector3 max, Vector3 sweep, float maximumT, BufferPool pool, Shapes shapes, void* overlaps) + where TOverlaps : ICollisionTaskSubpairOverlaps + { + Inner.FindLocalOverlaps(min, max, sweep, maximumT, pool, shapes, overlaps); + } + + public readonly void FindLocalOverlaps(Vector3 min, Vector3 max, BufferPool pool, Shapes shapes, ref TEnumerator enumerator) + where TEnumerator : IBreakableForEach + { + Inner.FindLocalOverlaps(min, max, pool, shapes, ref enumerator); + } + + public void Dispose(BufferPool pool) + { + Inner.Dispose(pool); + } +} + +/// +/// Drops convex shapes onto two WrappedMesh heightfields side by side. The fine mesh (many small triangles) forces MeshReduction into its +/// dictionary-based high-subpair-count path; the coarse mesh (few large triangles) keeps subpair counts under the brute-force threshold. +/// Between them the demo exercises every branch of for a non- +/// IHomogeneousCompoundShape so boundary smoothing can be validated on the type-erased path. +/// +public class CustomMeshSmoothingTestDemo : Demo +{ + (StaticHandle Handle, Mesh InnerMesh)[] wrappedMeshes; + + public override void Initialize(ContentArchive content, Camera camera) + { + camera.Position = new Vector3(0, 20, 60); + camera.Yaw = 0; + camera.Pitch = -0.3f; + + Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(new SpringSettings(30, 1)), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new SolveDescription(8, 1)); + + //Register collision tasks for every convex shape we're going to drop against the WrappedMesh. + //These are the same tasks DefaultTypes registers for Mesh, just closed over WrappedMesh so MeshReductionThunks is used instead of MeshReductionThunks. + var collisionTasks = Simulation.NarrowPhase.CollisionTaskRegistry; + collisionTasks.Register(new ConvexCompoundCollisionTask, ConvexMeshContinuations, MeshReduction>()); + collisionTasks.Register(new ConvexCompoundCollisionTask, ConvexMeshContinuations, MeshReduction>()); + collisionTasks.Register(new ConvexCompoundCollisionTask, ConvexMeshContinuations, MeshReduction>()); + collisionTasks.Register(new ConvexCompoundCollisionTask, ConvexMeshContinuations, MeshReduction>()); + collisionTasks.Register(new ConvexCompoundCollisionTask, ConvexMeshContinuations, MeshReduction>()); + collisionTasks.Register(new ConvexCompoundCollisionTask, ConvexMeshContinuations, MeshReduction>()); + + //Compound-vs-WrappedMesh uses a separate continuation type (CompoundMeshReduction), but it plugs into MeshReductionThunks the same way. + collisionTasks.Register(new CompoundPairCollisionTask, CompoundMeshContinuations, CompoundMeshReduction>()); + + //Sweep tasks matching the convex set, so swept queries keep working too. + var sweepTasks = Simulation.NarrowPhase.SweepTaskRegistry; + sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask>()); + sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask>()); + sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask>()); + sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask>()); + sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask>()); + sweepTasks.Register(new ConvexHomogeneousCompoundSweepTask>()); + sweepTasks.Register(new CompoundHomogeneousCompoundSweepTask>()); + + //Two meshes that share the same world-space terrain shape and footprint, but with wildly different tessellation density. + //The fine mesh pushes subpair counts into the dictionary path; the coarse mesh keeps them in the brute-force path. + wrappedMeshes = new (StaticHandle, Mesh)[2]; + var fineOrigin = Vector3.Zero; + var coarseOrigin = new Vector3(0, 0, 160); + AddWrappedTerrain(fineOrigin, planeWidth: 513, xzScale: 0.3f, out wrappedMeshes[0].Handle, out wrappedMeshes[0].InnerMesh); + AddShapesAt(fineOrigin); + AddWrappedTerrain(coarseOrigin, planeWidth: 33, xzScale: 4.8f, out wrappedMeshes[1].Handle, out wrappedMeshes[1].InnerMesh); + AddShapesAt(coarseOrigin); + } + + void AddWrappedTerrain(Vector3 staticPosition, int planeWidth, float xzScale, out StaticHandle handle, out Mesh innerMesh) + { + //The noise is evaluated in mesh-local world space so both meshes end up with the same apparent terrain — only triangle density differs. + Vector2 terrainOffset = new Vector2(1 - planeWidth, 1 - planeWidth) * 0.5f; + var scale = new Vector3(xzScale, 0.1f, xzScale); + innerMesh = DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth, + (int vX, int vY) => + { + //vX and vY are vertex indices; multiply by scale after adding the centering offset to get a local-space position in world units. + var localX = (vX + terrainOffset.X) * xzScale; + var localZ = (vY + terrainOffset.Y) * xzScale; + var octave0 = (MathF.Sin((localX + 5f) * 0.133f) + MathF.Sin((localZ + 11) * 0.133f)) * 0.9f; + var octave1 = (MathF.Sin((localX + 17) * 0.367f) + MathF.Sin((localZ + 19) * 0.367f)) * 0.35f; + var octave2 = (MathF.Sin((localX + 37) * 0.767f) + MathF.Sin((localZ + 93) * 0.767f)) * 0.15f; + var terrainHeight = octave0 + octave1 + octave2; + return new Vector3(vX + terrainOffset.X, terrainHeight, vY + terrainOffset.Y); + }, scale, BufferPool); + var wrapped = new WrappedMesh(innerMesh); + handle = Simulation.Statics.Add(new StaticDescription(staticPosition, QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2), Simulation.Shapes.Add(wrapped))); + } + + void AddShapesAt(Vector3 center) + { + //Wide, shallow shapes maximize the number of triangle AABBs intersecting the convex AABB on the fine mesh; on the coarse mesh the same shapes + //keep subpair counts well below MeshReduction's bruteForceThreshold of 128. + + //1) Small box: fewer than 128 subpairs on either mesh. + { + var box = new Box(1.2f, 1.2f, 1.2f); + var shape = Simulation.Shapes.Add(box); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(-12, 4, 0), box.ComputeInertia(1), shape, 0.01f)); + } + + //2) Medium box: ~300-500 subpairs on the fine mesh (dictionary path), a handful on the coarse mesh. + { + var box = new Box(5f, 0.6f, 5f); + var shape = Simulation.Shapes.Add(box); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(-4, 4, 0), box.ComputeInertia(1), shape, 0.01f)); + } + + //3) Large box: ~800-1000 subpairs on the fine mesh, still close to the skip threshold. + { + var box = new Box(8f, 0.6f, 8f); + var shape = Simulation.Shapes.Add(box); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(6, 4, 0), box.ComputeInertia(1), shape, 0.01f)); + } + + //4) Oversized box: intentionally exceeds the 1024-subpair skip threshold on the fine mesh to confirm the fall-through doesn't crash. + { + var box = new Box(14f, 0.6f, 14f); + var shape = Simulation.Shapes.Add(box); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(18, 4, 0), box.ComputeInertia(1), shape, 0.01f)); + } + + //5) A few rounded shapes rolling across the bumpy surface. Boundary smoothing matters most when contacts straddle edges, so rollers are a good stress test. + { + var sphere = new Sphere(1.5f); + var shape = Simulation.Shapes.Add(sphere); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(-12, 6, 6), sphere.ComputeInertia(1), shape, 0.01f)); + + var cylinder = new Cylinder(2.5f, 1.5f); + var cylinderShape = Simulation.Shapes.Add(cylinder); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(-4, 6, 6), cylinder.ComputeInertia(1), cylinderShape, 0.01f)); + + var capsule = new Capsule(0.8f, 4f); + var capsuleShape = Simulation.Shapes.Add(capsule); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(6, 6, 6), capsule.ComputeInertia(1), capsuleShape, 0.01f)); + } + + //6) A Compound of a few boxes. This routes through CompoundMeshContinuations / CompoundMeshReduction instead of the convex-only MeshReduction path, + // but it still feeds MeshReductionThunks, so it's the complementary check that compound-vs-wrapped-mesh boundary smoothing works too. + { + var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 3); + builder.Add(new Box(3f, 0.5f, 3f), RigidPose.Identity, 1); + builder.Add(new Box(1.5f, 1.5f, 1.5f), new RigidPose(new Vector3(0, 1f, 0)), 1); + builder.Add(new Box(0.75f, 0.75f, 4f), new RigidPose(new Vector3(1.5f, 0.5f, 0)), 1); + builder.BuildDynamicCompound(out var children, out var compoundInertia); + builder.Dispose(); + var compound = new Compound(children); + var shape = Simulation.Shapes.Add(compound); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(14, 8, -6), compoundInertia, shape, 0.01f)); + } + + //7) A wide, low convex hull. Hulls exercise a different convex-triangle tester than boxes, so including one catches regressions specific to hull-triangle manifolds. + { + const int hullPoints = 32; + var points = new QuickList(hullPoints, BufferPool); + var random = new Random(5); + for (int i = 0; i < hullPoints; ++i) + { + var xz = new Vector2(random.NextSingle() * 2 - 1, random.NextSingle() * 2 - 1); + //Flatten the hull so it covers a lot of ground when resting. + points.AllocateUnsafely() = new Vector3(xz.X * 3f, (random.NextSingle() * 2 - 1) * 0.35f, xz.Y * 3f); + } + var hull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _); + var shape = Simulation.Shapes.Add(hull); + Simulation.Bodies.Add(BodyDescription.CreateDynamic(center + new Vector3(-4, 8, -6), hull.ComputeInertia(1), shape, 0.01f)); + } + } + + public override void Render(Renderer renderer, Camera camera, Input input, TextBuilder text, Font font) + { + //The renderer's shape extractor switch doesn't know about WrappedMesh, so add each inner Mesh directly at its static's pose. + //Using AddShape (rather than AddShape) makes AddShape see Mesh.Id and routes to the existing mesh path. + foreach (var (handle, innerMesh) in wrappedMeshes) + { + ref var pose = ref Simulation.Statics[handle].Pose; + renderer.Shapes.AddShape(innerMesh, Simulation.Shapes, pose, new Vector3(0.7f, 0.7f, 0.75f)); + } + + var resolution = renderer.Surface.Resolution; + renderer.TextBatcher.Write(text.Clear().Append("Two WrappedMesh terrains: fine (near) and coarse (far, +Z). Identical shapes are dropped on each."), new Vector2(16, resolution.Y - 80), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Fine mesh pushes MeshReduction into its dictionary path; coarse mesh keeps everything in the brute-force path."), new Vector2(16, resolution.Y - 64), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("Note: the largest box on the fine mesh overlaps more than 1024 triangles, so MeshReduction.ReduceManifolds early-outs"), new Vector2(16, resolution.Y - 40), 16, Vector3.One, font); + renderer.TextBatcher.Write(text.Clear().Append("and no boundary smoothing is applied to it. Expect visible bumps there; the coarse-mesh counterpart still smooths."), new Vector2(16, resolution.Y - 24), 16, Vector3.One, font); + base.Render(renderer, camera, input, text, font); + } +}