diff --git a/BepuPhysics/Simulation.cs b/BepuPhysics/Simulation.cs index 0b5a065e..2cb5b61c 100644 --- a/BepuPhysics/Simulation.cs +++ b/BepuPhysics/Simulation.cs @@ -11,369 +11,406 @@ [module: SkipLocalsInit] #endif -namespace BepuPhysics +namespace BepuPhysics; + +/// +/// Orchestrates the bookkeeping and execution of a full dynamic simulation. +/// +public partial class Simulation : IDisposable { + /// + /// Gets the system responsible for awakening bodies within the simulation. + /// + public IslandAwakener Awakener { get; private set; } + /// + /// Gets the system responsible for putting bodies to sleep within the simulation. + /// + public IslandSleeper Sleeper { get; private set; } + /// + /// Gets the collection of bodies in the simulation. + /// + public Bodies Bodies { get; private set; } + /// + /// Gets the collection of statics within the simulation. + /// + public Statics Statics { get; private set; } + /// + /// Gets or sets the collection of shapes used by the simulation. + /// + /// + /// While instances can be shared between multiple instances, there are no guarantees of thread safety. + /// Further, setting the property to another collection while there are any outstanding references in the simulation to shapes within the old collection will result in fatal errors if the simulation continues to be used. + /// + public Shapes Shapes { get; set; } + /// + /// Gets the batch compressor used to compact constraints into fewer solver batches. + /// + public BatchCompressor SolverBatchCompressor { get; private set; } + /// + /// Gets the solver used to solved constraints within the simulation. + /// + public Solver Solver { get; private set; } + /// + /// Gets the integrator used to update velocities and poses within the simulation. + /// + public IPoseIntegrator PoseIntegrator { get; private set; } + /// + /// Gets the broad phase used by the simulation. Supports accelerated ray and volume queries. + /// + public BroadPhase BroadPhase { get; private set; } + /// + /// Gets the system used to find overlapping pairs within the simulation. + /// + public CollidableOverlapFinder BroadPhaseOverlapFinder { get; private set; } + /// + /// Gets the system used to identify contacts in colliding pairs of shapes and to update contact constraint data. + /// + public NarrowPhase NarrowPhase { get; private set; } + + SimulationProfiler profiler = new(13); + /// + /// Gets the simulation profiler. Note that the SimulationProfiler implementation only exists when the library is compiled with the PROFILE compilation symbol; if not defined, returned times are undefined. + /// + public SimulationProfiler Profiler { get { return profiler; } } + + //Helpers shared across at least two stages. + internal ConstraintRemover constraintRemover; + + /// + /// Gets the main memory pool used to fill persistent structures and main thread ephemeral resources across the engine. + /// + public BufferPool BufferPool { get; private set; } + + /// + /// Gets the timestepper used to update the simulation state. + /// + public ITimestepper Timestepper { get; private set; } + + /// + /// Gets or sets whether to use a deterministic time step when using multithreading. When set to true, additional time is spent sorting constraint additions and transfers. + /// Note that this can only affect determinism locally- different processor architectures may implement instructions differently. + /// + public bool Deterministic { get; set; } + /// - /// Orchestrates the bookkeeping and execution of a full dynamic simulation. + /// Constructs a simulation supporting dynamic movement and constraints with the specified narrow phase callbacks. /// - public partial class Simulation : IDisposable + /// Buffer pool used to fill persistent structures and main thread ephemeral resources across the engine. + /// Callbacks to use in the narrow phase. + /// Callbacks to use in the pose integrator. + /// Timestepper that defines how the simulation state should be updated. If null, is used. + /// Describes how the solver should execute, including the number of substeps and the number of velocity iterations per substep. + /// Allocation sizes to initialize the simulation with. If left null, default values are chosen. + /// Collection of shapes to use in the simulation, if any. If null, a new collection will be created for this simulation. + /// New simulation. + public static Simulation Create( + BufferPool bufferPool, TNarrowPhaseCallbacks narrowPhaseCallbacks, TPoseIntegratorCallbacks poseIntegratorCallbacks, SolveDescription solveDescription, ITimestepper timestepper = null, SimulationAllocationSizes? initialAllocationSizes = null, Shapes shapes = null) + where TNarrowPhaseCallbacks : struct, INarrowPhaseCallbacks + where TPoseIntegratorCallbacks : struct, IPoseIntegratorCallbacks { - public IslandAwakener Awakener { get; private set; } - public IslandSleeper Sleeper { get; private set; } - public Bodies Bodies { get; private set; } - public Statics Statics { get; private set; } - public Shapes Shapes { get; private set; } - public BatchCompressor SolverBatchCompressor { get; private set; } - public Solver Solver { get; private set; } - public IPoseIntegrator PoseIntegrator { get; private set; } - public BroadPhase BroadPhase { get; private set; } - public CollidableOverlapFinder BroadPhaseOverlapFinder { get; private set; } - public NarrowPhase NarrowPhase { get; private set; } - - SimulationProfiler profiler = new(13); - /// - /// Gets the simulation profiler. Note that the SimulationProfiler implementation only exists when the library is compiled with the PROFILE compilation symbol; if not defined, returned times are undefined. - /// - public SimulationProfiler Profiler { get { return profiler; } } - - //Helpers shared across at least two stages. - internal ConstraintRemover constraintRemover; - - /// - /// Gets the main memory pool used to fill persistent structures and main thread ephemeral resources across the engine. - /// - public BufferPool BufferPool { get; private set; } - - /// - /// Gets the timestepper used to update the simulation state. - /// - public ITimestepper Timestepper { get; private set; } - - /// - /// Gets or sets whether to use a deterministic time step when using multithreading. When set to true, additional time is spent sorting constraint additions and transfers. - /// Note that this can only affect determinism locally- different processor architectures may implement instructions differently. - /// - public bool Deterministic { get; set; } - - /// - /// Constructs a simulation supporting dynamic movement and constraints with the specified narrow phase callbacks. - /// - /// Buffer pool used to fill persistent structures and main thread ephemeral resources across the engine. - /// Callbacks to use in the narrow phase. - /// Callbacks to use in the pose integrator. - /// Timestepper that defines how the simulation state should be updated. If null, is used. - /// Describes how the solver should execute, including the number of substeps and the number of velocity iterations per substep. - /// Allocation sizes to initialize the simulation with. If left null, default values are chosen. - /// New simulation. - public static Simulation Create( - BufferPool bufferPool, TNarrowPhaseCallbacks narrowPhaseCallbacks, TPoseIntegratorCallbacks poseIntegratorCallbacks, SolveDescription solveDescription, ITimestepper timestepper = null, SimulationAllocationSizes? initialAllocationSizes = null) - where TNarrowPhaseCallbacks : struct, INarrowPhaseCallbacks - where TPoseIntegratorCallbacks : struct, IPoseIntegratorCallbacks + if (initialAllocationSizes == null) { - if (initialAllocationSizes == null) + initialAllocationSizes = new SimulationAllocationSizes { - initialAllocationSizes = new SimulationAllocationSizes - { - Bodies = 4096, - Statics = 4096, - ShapesPerType = 128, - ConstraintCountPerBodyEstimate = 8, - Constraints = 16384, - ConstraintsPerTypeBatch = 256 - }; - } - - //var simulation = new Simulation(bufferPool, initialAllocationSizes.Value, solverIterationCount, solverFallbackBatchThreshold, timestepper); - var simulation = new Simulation(); - simulation.BufferPool = bufferPool; - simulation.Shapes = new Shapes(bufferPool, initialAllocationSizes.Value.ShapesPerType); - simulation.BroadPhase = new BroadPhase(bufferPool, initialAllocationSizes.Value.Bodies, initialAllocationSizes.Value.Bodies + initialAllocationSizes.Value.Statics); - simulation.Bodies = new Bodies(bufferPool, simulation.Shapes, simulation.BroadPhase, - initialAllocationSizes.Value.Bodies, - initialAllocationSizes.Value.Islands, - initialAllocationSizes.Value.ConstraintCountPerBodyEstimate); - simulation.Statics = new Statics(bufferPool, simulation.Shapes, simulation.Bodies, simulation.BroadPhase, initialAllocationSizes.Value.Statics); - - var poseIntegrator = new PoseIntegrator(simulation.Bodies, simulation.Shapes, simulation.BroadPhase, poseIntegratorCallbacks); - simulation.PoseIntegrator = poseIntegrator; - - simulation.Solver = new Solver(simulation.Bodies, simulation.BufferPool, solveDescription, - initialCapacity: initialAllocationSizes.Value.Constraints, - initialIslandCapacity: initialAllocationSizes.Value.Islands, - minimumCapacityPerTypeBatch: initialAllocationSizes.Value.ConstraintsPerTypeBatch, poseIntegrator); - simulation.constraintRemover = new ConstraintRemover(simulation.BufferPool, simulation.Bodies, simulation.Solver); - simulation.Sleeper = new IslandSleeper(simulation.Bodies, simulation.Solver, simulation.BroadPhase, simulation.constraintRemover, simulation.BufferPool); - simulation.Awakener = new IslandAwakener(simulation.Bodies, simulation.Statics, simulation.Solver, simulation.BroadPhase, simulation.Sleeper, bufferPool); - simulation.Statics.awakener = simulation.Awakener; - simulation.Solver.awakener = simulation.Awakener; - simulation.Bodies.Initialize(simulation.Solver, simulation.Awakener, simulation.Sleeper); - simulation.SolverBatchCompressor = new BatchCompressor(simulation.Solver, simulation.Bodies); - simulation.Timestepper = timestepper ?? new DefaultTimestepper(); - - var narrowPhase = new NarrowPhase(simulation, - DefaultTypes.CreateDefaultCollisionTaskRegistry(), DefaultTypes.CreateDefaultSweepTaskRegistry(), - narrowPhaseCallbacks, initialAllocationSizes.Value.Islands + 1); - DefaultTypes.RegisterDefaults(simulation.Solver, narrowPhase); - simulation.NarrowPhase = narrowPhase; - simulation.Sleeper.pairCache = narrowPhase.PairCache; - simulation.Awakener.pairCache = narrowPhase.PairCache; - simulation.Solver.pairCache = narrowPhase.PairCache; - simulation.BroadPhaseOverlapFinder = new CollidableOverlapFinder(narrowPhase, simulation.BroadPhase); - - //We defer initialization until after all the other simulation bits are constructed. - poseIntegrator.Callbacks.Initialize(simulation); - narrowPhase.Callbacks.Initialize(simulation); - - return simulation; + Bodies = 4096, + Statics = 4096, + ShapesPerType = 128, + ConstraintCountPerBodyEstimate = 8, + Constraints = 16384, + ConstraintsPerTypeBatch = 256 + }; } + //var simulation = new Simulation(bufferPool, initialAllocationSizes.Value, solverIterationCount, solverFallbackBatchThreshold, timestepper); + var simulation = new Simulation(); + simulation.BufferPool = bufferPool; + simulation.Shapes = shapes == null ? new Shapes(bufferPool, initialAllocationSizes.Value.ShapesPerType) : shapes; + simulation.BroadPhase = new BroadPhase(bufferPool, initialAllocationSizes.Value.Bodies, initialAllocationSizes.Value.Bodies + initialAllocationSizes.Value.Statics); + simulation.Bodies = new Bodies(bufferPool, simulation.Shapes, simulation.BroadPhase, + initialAllocationSizes.Value.Bodies, + initialAllocationSizes.Value.Islands, + initialAllocationSizes.Value.ConstraintCountPerBodyEstimate); + simulation.Statics = new Statics(bufferPool, simulation.Shapes, simulation.Bodies, simulation.BroadPhase, initialAllocationSizes.Value.Statics); + + var poseIntegrator = new PoseIntegrator(simulation.Bodies, simulation.Shapes, simulation.BroadPhase, poseIntegratorCallbacks); + simulation.PoseIntegrator = poseIntegrator; + + simulation.Solver = new Solver(simulation.Bodies, simulation.BufferPool, solveDescription, + initialCapacity: initialAllocationSizes.Value.Constraints, + initialIslandCapacity: initialAllocationSizes.Value.Islands, + minimumCapacityPerTypeBatch: initialAllocationSizes.Value.ConstraintsPerTypeBatch, poseIntegrator); + simulation.constraintRemover = new ConstraintRemover(simulation.BufferPool, simulation.Bodies, simulation.Solver); + simulation.Sleeper = new IslandSleeper(simulation.Bodies, simulation.Solver, simulation.BroadPhase, simulation.constraintRemover, simulation.BufferPool); + simulation.Awakener = new IslandAwakener(simulation.Bodies, simulation.Statics, simulation.Solver, simulation.BroadPhase, simulation.Sleeper, bufferPool); + simulation.Statics.awakener = simulation.Awakener; + simulation.Solver.awakener = simulation.Awakener; + simulation.Bodies.Initialize(simulation.Solver, simulation.Awakener, simulation.Sleeper); + simulation.SolverBatchCompressor = new BatchCompressor(simulation.Solver, simulation.Bodies); + simulation.Timestepper = timestepper ?? new DefaultTimestepper(); + + var narrowPhase = new NarrowPhase(simulation, + DefaultTypes.CreateDefaultCollisionTaskRegistry(), DefaultTypes.CreateDefaultSweepTaskRegistry(), + narrowPhaseCallbacks, initialAllocationSizes.Value.Islands + 1); + DefaultTypes.RegisterDefaults(simulation.Solver, narrowPhase); + simulation.NarrowPhase = narrowPhase; + simulation.Sleeper.pairCache = narrowPhase.PairCache; + simulation.Awakener.pairCache = narrowPhase.PairCache; + simulation.Solver.pairCache = narrowPhase.PairCache; + simulation.BroadPhaseOverlapFinder = new CollidableOverlapFinder(narrowPhase, simulation.BroadPhase); + + //We defer initialization until after all the other simulation bits are constructed. + poseIntegrator.Callbacks.Initialize(simulation); + narrowPhase.Callbacks.Initialize(simulation); + + return simulation; + } + - private static int ValidateAndCountShapefulBodies(ref BodySet bodySet, ref Tree tree, ref Buffer leaves) + private static int ValidateAndCountShapefulBodies(ref BodySet bodySet, ref Tree tree, ref Buffer leaves) + { + int shapefulBodyCount = 0; + for (int i = 0; i < bodySet.Count; ++i) { - int shapefulBodyCount = 0; - for (int i = 0; i < bodySet.Count; ++i) + ref var collidable = ref bodySet.Collidables[i]; + if (collidable.Shape.Exists) { - ref var collidable = ref bodySet.Collidables[i]; - if (collidable.Shape.Exists) - { - Debug.Assert(collidable.BroadPhaseIndex >= 0 && collidable.BroadPhaseIndex < tree.LeafCount); - ref var leaf = ref leaves[collidable.BroadPhaseIndex]; - Debug.Assert(leaf.StaticHandle.Value == bodySet.IndexToHandle[i].Value); - Debug.Assert(leaf.Mobility == CollidableMobility.Dynamic || leaf.Mobility == CollidableMobility.Kinematic); - ++shapefulBodyCount; - } + Debug.Assert(collidable.BroadPhaseIndex >= 0 && collidable.BroadPhaseIndex < tree.LeafCount); + ref var leaf = ref leaves[collidable.BroadPhaseIndex]; + Debug.Assert(leaf.StaticHandle.Value == bodySet.IndexToHandle[i].Value); + Debug.Assert(leaf.Mobility == CollidableMobility.Dynamic || leaf.Mobility == CollidableMobility.Kinematic); + ++shapefulBodyCount; } - return shapefulBodyCount; } + return shapefulBodyCount; + } - [Conditional("DEBUG")] - internal void ValidateCollidables() - { - var activeShapefulBodyCount = ValidateAndCountShapefulBodies(ref Bodies.ActiveSet, ref BroadPhase.ActiveTree, ref BroadPhase.ActiveLeaves); - Debug.Assert(BroadPhase.ActiveTree.LeafCount == activeShapefulBodyCount); + [Conditional("DEBUG")] + internal void ValidateCollidables() + { + var activeShapefulBodyCount = ValidateAndCountShapefulBodies(ref Bodies.ActiveSet, ref BroadPhase.ActiveTree, ref BroadPhase.ActiveLeaves); + Debug.Assert(BroadPhase.ActiveTree.LeafCount == activeShapefulBodyCount); - int inactiveShapefulBodyCount = 0; + int inactiveShapefulBodyCount = 0; - for (int setIndex = 1; setIndex < Bodies.Sets.Length; ++setIndex) + for (int setIndex = 1; setIndex < Bodies.Sets.Length; ++setIndex) + { + ref var set = ref Bodies.Sets[setIndex]; + if (set.Allocated) { - ref var set = ref Bodies.Sets[setIndex]; - if (set.Allocated) - { - inactiveShapefulBodyCount += ValidateAndCountShapefulBodies(ref set, ref BroadPhase.StaticTree, ref BroadPhase.StaticLeaves); - } + inactiveShapefulBodyCount += ValidateAndCountShapefulBodies(ref set, ref BroadPhase.StaticTree, ref BroadPhase.StaticLeaves); } - Debug.Assert(inactiveShapefulBodyCount + Statics.Count == BroadPhase.StaticTree.LeafCount); - for (int i = 0; i < Statics.Count; ++i) - { - ref var collidable = ref Statics[i]; - Debug.Assert(collidable.Shape.Exists, "All static collidables must have shapes. That's their only purpose."); + } + Debug.Assert(inactiveShapefulBodyCount + Statics.Count == BroadPhase.StaticTree.LeafCount); + for (int i = 0; i < Statics.Count; ++i) + { + ref var collidable = ref Statics[i]; + Debug.Assert(collidable.Shape.Exists, "All static collidables must have shapes. That's their only purpose."); - Debug.Assert(collidable.BroadPhaseIndex >= 0 && collidable.BroadPhaseIndex < BroadPhase.StaticTree.LeafCount); - ref var leaf = ref BroadPhase.StaticLeaves[collidable.BroadPhaseIndex]; - Debug.Assert(leaf.StaticHandle.Value == Statics.IndexToHandle[i].Value); - Debug.Assert(leaf.Mobility == CollidableMobility.Static); - } + Debug.Assert(collidable.BroadPhaseIndex >= 0 && collidable.BroadPhaseIndex < BroadPhase.StaticTree.LeafCount); + ref var leaf = ref BroadPhase.StaticLeaves[collidable.BroadPhaseIndex]; + Debug.Assert(leaf.StaticHandle.Value == Statics.IndexToHandle[i].Value); + Debug.Assert(leaf.Mobility == CollidableMobility.Static); + } - //Ensure there are no duplicates between the two broad phase trees. - for (int i = 0; i < BroadPhase.ActiveTree.LeafCount; ++i) + //Ensure there are no duplicates between the two broad phase trees. + for (int i = 0; i < BroadPhase.ActiveTree.LeafCount; ++i) + { + var activeLeaf = BroadPhase.ActiveLeaves[i]; + for (int j = 0; j < BroadPhase.StaticTree.LeafCount; ++j) { - var activeLeaf = BroadPhase.ActiveLeaves[i]; - for (int j = 0; j < BroadPhase.StaticTree.LeafCount; ++j) - { - Debug.Assert(BroadPhase.StaticLeaves[j].Packed != activeLeaf.Packed); - } + Debug.Assert(BroadPhase.StaticLeaves[j].Packed != activeLeaf.Packed); } - } - //These functions act as convenience wrappers around common execution patterns. They can be mixed and matched in custom timesteps, or for certain advanced use cases, called directly. - /// - /// Executes the sleep stage, moving candidate - /// - /// Thread dispatcher to use for the sleeper execution, if any. - public void Sleep(IThreadDispatcher threadDispatcher = null) - { - profiler.Start(Sleeper); - Sleeper.Update(threadDispatcher, Deterministic); - profiler.End(Sleeper); - } + } - /// - /// Predicts the bounding boxes of active bodies by speculatively integrating velocity. Does not actually modify body velocities. Updates deactivation candidacy. - /// - /// Duration of the time step. - /// Thread dispatcher to use for execution, if any. - public void PredictBoundingBoxes(float dt, IThreadDispatcher threadDispatcher = null) - { - profiler.Start(PoseIntegrator); - PoseIntegrator.PredictBoundingBoxes(dt, BufferPool, threadDispatcher); - profiler.End(PoseIntegrator); - } + //These functions act as convenience wrappers around common execution patterns. They can be mixed and matched in custom timesteps, or for certain advanced use cases, called directly. + /// + /// Executes the sleep stage, moving candidate + /// + /// Thread dispatcher to use for the sleeper execution, if any. + public void Sleep(IThreadDispatcher threadDispatcher = null) + { + profiler.Start(Sleeper); + Sleeper.Update(threadDispatcher, Deterministic); + profiler.End(Sleeper); + } - /// - /// Updates the broad phase structure for the current body bounding boxes, finds potentially colliding pairs, and then executes the narrow phase for all such pairs. Generates contact constraints for the solver. - /// - /// Duration of the time step. - /// Thread dispatcher to use for execution, if any. - public void CollisionDetection(float dt, IThreadDispatcher threadDispatcher = null) - { - profiler.Start(BroadPhase); - //BroadPhase.Update(threadDispatcher); - BroadPhase.Update2(threadDispatcher); - profiler.End(BroadPhase); - - profiler.Start(BroadPhaseOverlapFinder); - BroadPhaseOverlapFinder.DispatchOverlaps(dt, threadDispatcher); - profiler.End(BroadPhaseOverlapFinder); - - profiler.Start(NarrowPhase); - NarrowPhase.Flush(threadDispatcher); - profiler.End(NarrowPhase); - } + /// + /// Predicts the bounding boxes of active bodies by speculatively integrating velocity. Does not actually modify body velocities. Updates deactivation candidacy. + /// + /// Duration of the time step. + /// Thread dispatcher to use for execution, if any. + public void PredictBoundingBoxes(float dt, IThreadDispatcher threadDispatcher = null) + { + profiler.Start(PoseIntegrator); + PoseIntegrator.PredictBoundingBoxes(dt, BufferPool, threadDispatcher); + profiler.End(PoseIntegrator); + } - /// - /// Updates the broad phase structure for the current body bounding boxes, finds potentially colliding pairs, and then executes the narrow phase for all such pairs. Generates contact constraints for the solver. - /// - /// Duration of the time step. - /// Thread dispatcher to use for execution, if any. - public void Solve(float dt, IThreadDispatcher threadDispatcher = null) - { - Profiler.Start(Solver); - var constrainedBodySet = Solver.PrepareConstraintIntegrationResponsibilities(threadDispatcher); - Solver.Solve(dt, threadDispatcher); - Profiler.End(Solver); + /// + /// Updates the broad phase structure for the current body bounding boxes, finds potentially colliding pairs, and then executes the narrow phase for all such pairs. Generates contact constraints for the solver. + /// + /// Duration of the time step. + /// Thread dispatcher to use for execution, if any. + public void CollisionDetection(float dt, IThreadDispatcher threadDispatcher = null) + { + profiler.Start(BroadPhase); + //BroadPhase.Update(threadDispatcher); + BroadPhase.Update2(threadDispatcher); + profiler.End(BroadPhase); + + profiler.Start(BroadPhaseOverlapFinder); + BroadPhaseOverlapFinder.DispatchOverlaps(dt, threadDispatcher); + profiler.End(BroadPhaseOverlapFinder); + + profiler.Start(NarrowPhase); + NarrowPhase.Flush(threadDispatcher); + profiler.End(NarrowPhase); + } - Profiler.Start(PoseIntegrator); - PoseIntegrator.IntegrateAfterSubstepping(constrainedBodySet, dt, Solver.SubstepCount, threadDispatcher); - Profiler.End(PoseIntegrator); + /// + /// Updates the broad phase structure for the current body bounding boxes, finds potentially colliding pairs, and then executes the narrow phase for all such pairs. Generates contact constraints for the solver. + /// + /// Duration of the time step. + /// Thread dispatcher to use for execution, if any. + public void Solve(float dt, IThreadDispatcher threadDispatcher = null) + { + Profiler.Start(Solver); + var constrainedBodySet = Solver.PrepareConstraintIntegrationResponsibilities(threadDispatcher); + Solver.Solve(dt, threadDispatcher); + Profiler.End(Solver); - Solver.DisposeConstraintIntegrationResponsibilities(); - } + Profiler.Start(PoseIntegrator); + PoseIntegrator.IntegrateAfterSubstepping(constrainedBodySet, dt, Solver.SubstepCount, threadDispatcher); + Profiler.End(PoseIntegrator); - /// - /// Incrementally improves body and constraint storage for better performance. - /// - /// Thread dispatcher to use for execution, if any. - public void IncrementallyOptimizeDataStructures(IThreadDispatcher threadDispatcher = null) - { - //Previously, this handled body and constraint memory layout optimization. 2.4 significantly changed how memory accesses work in the solver - //and the optimizers were no longer net wins, so all that's left is the batch compressor. - //It pulls constraints currently living in high constraint batch indices to lower constraint batches if possible. - //Over time, that'll tend to reduce sync points in the solver and improve performance. - profiler.Start(SolverBatchCompressor); - SolverBatchCompressor.Compress(BufferPool, threadDispatcher, threadDispatcher != null && Deterministic); - profiler.End(SolverBatchCompressor); - } + Solver.DisposeConstraintIntegrationResponsibilities(); + } - //TODO: I wonder if people will abuse the dt-as-parameter to the point where we should make it a field instead, like it effectively was in v1. - /// - /// Performs one timestep of the given length. - /// - /// - /// Be wary of variable timesteps. They can harm stability. Whenever possible, keep the timestep the same across multiple frames unless you have a specific reason not to. - /// - /// Duration of the time step. - /// Thread dispatcher to use for execution, if any. - public void Timestep(float dt, IThreadDispatcher threadDispatcher = null) - { - if (dt <= 0) - throw new ArgumentException("Timestep duration must be positive.", nameof(dt)); - profiler.Clear(); - profiler.Start(this); + /// + /// Incrementally improves body and constraint storage for better performance. + /// + /// Thread dispatcher to use for execution, if any. + public void IncrementallyOptimizeDataStructures(IThreadDispatcher threadDispatcher = null) + { + //Previously, this handled body and constraint memory layout optimization. 2.4 significantly changed how memory accesses work in the solver + //and the optimizers were no longer net wins, so all that's left is the batch compressor. + //It pulls constraints currently living in high constraint batch indices to lower constraint batches if possible. + //Over time, that'll tend to reduce sync points in the solver and improve performance. + profiler.Start(SolverBatchCompressor); + SolverBatchCompressor.Compress(BufferPool, threadDispatcher, threadDispatcher != null && Deterministic); + profiler.End(SolverBatchCompressor); + } - Timestepper.Timestep(this, dt, threadDispatcher); + //TODO: I wonder if people will abuse the dt-as-parameter to the point where we should make it a field instead, like it effectively was in v1. + /// + /// Performs one timestep of the given length. + /// + /// + /// Be wary of variable timesteps. They can harm stability. Whenever possible, keep the timestep the same across multiple frames unless you have a specific reason not to. + /// + /// Duration of the time step. + /// Thread dispatcher to use for execution, if any. + public void Timestep(float dt, IThreadDispatcher threadDispatcher = null) + { + if (dt <= 0) + throw new ArgumentException("Timestep duration must be positive.", nameof(dt)); + profiler.Clear(); + profiler.Start(this); - profiler.End(this); - } + Timestepper.Timestep(this, dt, threadDispatcher); - /// - /// Clears the simulation of every object, only returning memory to the pool that would be returned by sequential removes. - /// Other persistent allocations, like those in the Bodies set, will remain. - /// - public void Clear() - { - Solver.Clear(); - Bodies.Clear(); - Statics.Clear(); - Shapes.Clear(); - BroadPhase.Clear(); - NarrowPhase.Clear(); - Sleeper.Clear(); - } + profiler.End(this); + } - /// - /// Increases the allocation size of any buffers too small to hold the allocation target. - /// - /// - /// - /// The final size of the allocated buffers are constrained by the allocator. It is not guaranteed to be exactly equal to the target, but it is guaranteed to be at least as large. - /// - /// - /// This is primarily a convenience function. Everything it does internally can be done externally. - /// For example, if only type batches need to be resized, the solver's own functions can be used directly. - /// - /// - /// Allocation sizes to guarantee sufficient size for. - public void EnsureCapacity(SimulationAllocationSizes allocationTarget) - { - Solver.EnsureSolverCapacities(allocationTarget.Bodies, allocationTarget.Constraints); - Solver.MinimumCapacityPerTypeBatch = Math.Max(allocationTarget.ConstraintsPerTypeBatch, Solver.MinimumCapacityPerTypeBatch); - Solver.EnsureTypeBatchCapacities(); - NarrowPhase.PairCache.EnsureConstraintToPairMappingCapacity(Solver, allocationTarget.Constraints); - //Note that the bodies set has to come before the body layout optimizer; the body layout optimizer's sizes are dependent upon the bodies set. - Bodies.EnsureCapacity(allocationTarget.Bodies); - Bodies.MinimumConstraintCapacityPerBody = allocationTarget.ConstraintCountPerBodyEstimate; - Bodies.EnsureConstraintListCapacities(); - Sleeper.EnsureSetsCapacity(allocationTarget.Islands + 1); - Statics.EnsureCapacity(allocationTarget.Statics); - Shapes.EnsureBatchCapacities(allocationTarget.ShapesPerType); - BroadPhase.EnsureCapacity(allocationTarget.Bodies, allocationTarget.Bodies + allocationTarget.Statics); - } + /// + /// Clears the simulation of every object, only returning memory to the pool that would be returned by sequential removes. + /// Other persistent allocations, like those in the Bodies set, will remain. + /// + public void Clear() + { + Solver.Clear(); + Bodies.Clear(); + Statics.Clear(); + Shapes.Clear(); + BroadPhase.Clear(); + NarrowPhase.Clear(); + Sleeper.Clear(); + } + /// + /// Increases the allocation size of any buffers too small to hold the allocation target. + /// + /// + /// + /// The final size of the allocated buffers are constrained by the allocator. It is not guaranteed to be exactly equal to the target, but it is guaranteed to be at least as large. + /// + /// + /// This is primarily a convenience function. Everything it does internally can be done externally. + /// For example, if only type batches need to be resized, the solver's own functions can be used directly. + /// + /// + /// Allocation sizes to guarantee sufficient size for. + public void EnsureCapacity(SimulationAllocationSizes allocationTarget) + { + Solver.EnsureSolverCapacities(allocationTarget.Bodies, allocationTarget.Constraints); + Solver.MinimumCapacityPerTypeBatch = Math.Max(allocationTarget.ConstraintsPerTypeBatch, Solver.MinimumCapacityPerTypeBatch); + Solver.EnsureTypeBatchCapacities(); + NarrowPhase.PairCache.EnsureConstraintToPairMappingCapacity(Solver, allocationTarget.Constraints); + //Note that the bodies set has to come before the body layout optimizer; the body layout optimizer's sizes are dependent upon the bodies set. + Bodies.EnsureCapacity(allocationTarget.Bodies); + Bodies.MinimumConstraintCapacityPerBody = allocationTarget.ConstraintCountPerBodyEstimate; + Bodies.EnsureConstraintListCapacities(); + Sleeper.EnsureSetsCapacity(allocationTarget.Islands + 1); + Statics.EnsureCapacity(allocationTarget.Statics); + Shapes.EnsureBatchCapacities(allocationTarget.ShapesPerType); + BroadPhase.EnsureCapacity(allocationTarget.Bodies, allocationTarget.Bodies + allocationTarget.Statics); + } - /// - /// Increases the allocation size of any buffers too small to hold the allocation target, and decreases the allocation size of any buffers that are unnecessarily large. - /// - /// - /// - /// The final size of the allocated buffers are constrained by the allocator. It is not guaranteed to be exactly equal to the target, but it is guaranteed to be at least as large. - /// - /// - /// This is primarily a convenience function. Everything it does internally can be done externally. - /// For example, if only type batches need to be resized, the solver's own functions can be used directly. - /// - /// - /// Allocation sizes to guarantee sufficient size for. - public void Resize(SimulationAllocationSizes allocationTarget) - { - Solver.ResizeSolverCapacities(allocationTarget.Bodies, allocationTarget.Constraints); - Solver.MinimumCapacityPerTypeBatch = allocationTarget.ConstraintsPerTypeBatch; - Solver.ResizeTypeBatchCapacities(); - NarrowPhase.PairCache.ResizeConstraintToPairMappingCapacity(Solver, allocationTarget.Constraints); - //Note that the bodies set has to come before the body layout optimizer; the body layout optimizer's sizes are dependent upon the bodies set. - Bodies.Resize(allocationTarget.Bodies); - Bodies.MinimumConstraintCapacityPerBody = allocationTarget.ConstraintCountPerBodyEstimate; - Bodies.ResizeConstraintListCapacities(); - Sleeper.ResizeSetsCapacity(allocationTarget.Islands + 1); - Statics.Resize(allocationTarget.Statics); - Shapes.ResizeBatches(allocationTarget.ShapesPerType); - BroadPhase.Resize(allocationTarget.Bodies, allocationTarget.Bodies + allocationTarget.Statics); - } - /// - /// Clears the simulation of every object and returns all pooled memory to the buffer pool. Leaves the simulation in an unusable state. - /// - public void Dispose() - { - Clear(); - Sleeper.Dispose(); - Solver.Dispose(); - BroadPhase.Dispose(); - NarrowPhase.Dispose(); - Bodies.Dispose(); - Statics.Dispose(); - Shapes.Dispose(); - } + /// + /// Increases the allocation size of any buffers too small to hold the allocation target, and decreases the allocation size of any buffers that are unnecessarily large. + /// + /// + /// + /// The final size of the allocated buffers are constrained by the allocator. It is not guaranteed to be exactly equal to the target, but it is guaranteed to be at least as large. + /// + /// + /// This is primarily a convenience function. Everything it does internally can be done externally. + /// For example, if only type batches need to be resized, the solver's own functions can be used directly. + /// + /// + /// Allocation sizes to guarantee sufficient size for. + public void Resize(SimulationAllocationSizes allocationTarget) + { + Solver.ResizeSolverCapacities(allocationTarget.Bodies, allocationTarget.Constraints); + Solver.MinimumCapacityPerTypeBatch = allocationTarget.ConstraintsPerTypeBatch; + Solver.ResizeTypeBatchCapacities(); + NarrowPhase.PairCache.ResizeConstraintToPairMappingCapacity(Solver, allocationTarget.Constraints); + //Note that the bodies set has to come before the body layout optimizer; the body layout optimizer's sizes are dependent upon the bodies set. + Bodies.Resize(allocationTarget.Bodies); + Bodies.MinimumConstraintCapacityPerBody = allocationTarget.ConstraintCountPerBodyEstimate; + Bodies.ResizeConstraintListCapacities(); + Sleeper.ResizeSetsCapacity(allocationTarget.Islands + 1); + Statics.Resize(allocationTarget.Statics); + Shapes.ResizeBatches(allocationTarget.ShapesPerType); + BroadPhase.Resize(allocationTarget.Bodies, allocationTarget.Bodies + allocationTarget.Statics); + } + + /// + /// Clears the simulation of every object and returns all pooled memory to the buffer pool. Leaves the simulation in an unusable state. + /// + public void Dispose() + { + Clear(); + Sleeper.Dispose(); + Solver.Dispose(); + BroadPhase.Dispose(); + NarrowPhase.Dispose(); + Bodies.Dispose(); + Statics.Dispose(); + Shapes.Dispose(); } } diff --git a/BepuPhysics/SimulationAllocationSizes.cs b/BepuPhysics/SimulationAllocationSizes.cs index 1ac2f0a2..44536eef 100644 --- a/BepuPhysics/SimulationAllocationSizes.cs +++ b/BepuPhysics/SimulationAllocationSizes.cs @@ -1,69 +1,73 @@ -using System.Runtime.InteropServices; - -namespace BepuPhysics -{ - /// - /// The common set of allocation sizes for a simulation. - /// - [StructLayout(LayoutKind.Sequential)] - public struct SimulationAllocationSizes - { - /// - /// The number of bodies to allocate space for. - /// - public int Bodies; - /// - /// The number of statics to allocate space for. - /// - public int Statics; - /// - /// The number of inactive islands to allocate space for. - /// - public int Islands; - /// - /// Minimum number of shapes to allocate space for in each shape type batch. - /// - public int ShapesPerType; - /// - /// The number of constraints to allocate bookkeeping space for. This does not affect actual type batch allocation sizes, only the solver-level constraint handle storage. - /// - public int Constraints; - /// - /// The minimum number of constraints to allocate space for in each individual type batch. - /// New type batches will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. - /// The number of constraints can vary greatly across types- there are usually far more contacts than ragdoll constraints. - /// Per type estimates can be assigned within the Solver.TypeBatchAllocation if necessary. This value acts as a lower bound for all types. - /// - public int ConstraintsPerTypeBatch; - /// - /// The minimum number of constraints to allocate space for in each body's constraint list. - /// New bodies will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. - /// - public int ConstraintCountPerBodyEstimate; - - /// - /// Constructs a description of simulation allocations. - /// - /// The number of bodies to allocate space for. - /// The number of statics to allocate space for. - /// The number of inactive islands to allocate space for. - /// Minimum number of shapes to allocate space for in each shape type batch. - /// The number of constraints to allocate bookkeeping space for. This does not affect actual type batch allocation sizes, only the solver-level constraint handle storage. - /// The minimum number of constraints to allocate space for in each individual type batch. - /// New type batches will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. - /// The number of constraints can vary greatly across types- there are usually far more contacts than ragdoll constraints. - /// Per type estimates can be assigned within the Solver.TypeBatchAllocation if necessary. This value acts as a lower bound for all types. - /// The minimum number of constraints to allocate space for in each body's constraint list. - /// New bodies will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. - public SimulationAllocationSizes(int bodies, int statics, int islands, int shapesPerType, int constraints, int constraintsPerTypeBatch, int constraintCountPerBodyEstimate) - { - Bodies = bodies; - Statics = statics; - Islands = islands; - ShapesPerType = shapesPerType; - Constraints = constraints; - ConstraintsPerTypeBatch = constraintsPerTypeBatch; - ConstraintCountPerBodyEstimate = constraintCountPerBodyEstimate; - } - } -} +using BepuPhysics.Collidables; +using System.Runtime.InteropServices; + +namespace BepuPhysics +{ + /// + /// The common set of allocation sizes for a simulation. + /// + [StructLayout(LayoutKind.Sequential)] + public struct SimulationAllocationSizes + { + /// + /// The number of bodies to allocate space for. + /// + public int Bodies; + /// + /// The number of statics to allocate space for. + /// + public int Statics; + /// + /// The number of inactive islands to allocate space for. + /// + public int Islands; + /// + /// Minimum number of shapes to allocate space for in each shape type batch. + /// + /// + /// Unused if a instance was directly provided to the constructor. + /// + public int ShapesPerType; + /// + /// The number of constraints to allocate bookkeeping space for. This does not affect actual type batch allocation sizes, only the solver-level constraint handle storage. + /// + public int Constraints; + /// + /// The minimum number of constraints to allocate space for in each individual type batch. + /// New type batches will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. + /// The number of constraints can vary greatly across types- there are usually far more contacts than ragdoll constraints. + /// Per type estimates can be assigned within the Solver.TypeBatchAllocation if necessary. This value acts as a lower bound for all types. + /// + public int ConstraintsPerTypeBatch; + /// + /// The minimum number of constraints to allocate space for in each body's constraint list. + /// New bodies will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. + /// + public int ConstraintCountPerBodyEstimate; + + /// + /// Constructs a description of simulation allocations. + /// + /// The number of bodies to allocate space for. + /// The number of statics to allocate space for. + /// The number of inactive islands to allocate space for. + /// Minimum number of shapes to allocate space for in each shape type batch. Unused if a instance was directly provided to the constructor. + /// The number of constraints to allocate bookkeeping space for. This does not affect actual type batch allocation sizes, only the solver-level constraint handle storage. + /// The minimum number of constraints to allocate space for in each individual type batch. + /// New type batches will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. + /// The number of constraints can vary greatly across types- there are usually far more contacts than ragdoll constraints. + /// Per type estimates can be assigned within the Solver.TypeBatchAllocation if necessary. This value acts as a lower bound for all types. + /// The minimum number of constraints to allocate space for in each body's constraint list. + /// New bodies will be given enough memory for this number of constraints, and any compaction will not reduce the allocations below it. + public SimulationAllocationSizes(int bodies, int statics, int islands, int shapesPerType, int constraints, int constraintsPerTypeBatch, int constraintCountPerBodyEstimate) + { + Bodies = bodies; + Statics = statics; + Islands = islands; + ShapesPerType = shapesPerType; + Constraints = constraints; + ConstraintsPerTypeBatch = constraintsPerTypeBatch; + ConstraintCountPerBodyEstimate = constraintCountPerBodyEstimate; + } + } +}