public PoseIntegrator(Bodies bodies, Shapes shapes, BroadPhase broadPhase)
{
this.bodies = bodies;
this.shapes = shapes;
this.broadPhase = broadPhase;
workerDelegate = Worker;
}
public Simulation(BufferPool bufferPool, SimulationAllocationSizes initialAllocationSizes, int solverIterationCount, int solverFallbackBatchThreshold, ITimestepper timestepper)
{
BufferPool = bufferPool;
Shapes = new Shapes(bufferPool, initialAllocationSizes.ShapesPerType);
BroadPhase = new BroadPhase(bufferPool, initialAllocationSizes.Bodies, initialAllocationSizes.Bodies + initialAllocationSizes.Statics);
Bodies = new Bodies(bufferPool, Shapes, BroadPhase,
initialAllocationSizes.Bodies,
initialAllocationSizes.Islands,
initialAllocationSizes.ConstraintCountPerBodyEstimate);
Statics = new Statics(bufferPool, Shapes, Bodies, BroadPhase, initialAllocationSizes.Statics);
Solver = new Solver(Bodies, BufferPool, solverIterationCount, solverFallbackBatchThreshold,
initialCapacity: initialAllocationSizes.Constraints,
initialIslandCapacity: initialAllocationSizes.Islands,
minimumCapacityPerTypeBatch: initialAllocationSizes.ConstraintsPerTypeBatch);
constraintRemover = new ConstraintRemover(BufferPool, Bodies, Solver);
Sleeper = new IslandSleeper(Bodies, Solver, BroadPhase, constraintRemover, BufferPool);
Awakener = new IslandAwakener(Bodies, Statics, Solver, BroadPhase, Sleeper, bufferPool);
Statics.awakener = Awakener;
Solver.awakener = Awakener;
Bodies.Initialize(Solver, Awakener);
SolverBatchCompressor = new BatchCompressor(Solver, Bodies);
BodyLayoutOptimizer = new BodyLayoutOptimizer(Bodies, BroadPhase, Solver, bufferPool);
ConstraintLayoutOptimizer = new ConstraintLayoutOptimizer(Bodies, Solver);
Timestepper = timestepper;
}
public BodyLayoutOptimizer(Bodies bodies, BroadPhase broadPhase, Solver solver, BufferPool pool, float optimizationFraction = 0.005f)
{
this.bodies = bodies;
this.broadPhase = broadPhase;
this.solver = solver;
OptimizationFraction = optimizationFraction;
}
protected Simulation(BufferPool bufferPool, SimulationAllocationSizes initialAllocationSizes)
{
BufferPool = bufferPool;
Shapes = new Shapes(bufferPool, initialAllocationSizes.ShapesPerType);
BroadPhase = new BroadPhase(bufferPool, initialAllocationSizes.Bodies, initialAllocationSizes.Bodies + initialAllocationSizes.Statics);
Bodies = new Bodies(bufferPool, Shapes, BroadPhase,
initialAllocationSizes.Bodies,
initialAllocationSizes.Islands,
initialAllocationSizes.ConstraintCountPerBodyEstimate);
Statics = new Statics(bufferPool, Shapes, Bodies, BroadPhase, initialAllocationSizes.Statics);
Solver = new Solver(Bodies, BufferPool, 8,
initialCapacity: initialAllocationSizes.Constraints,
initialIslandCapacity: initialAllocationSizes.Islands,
minimumCapacityPerTypeBatch: initialAllocationSizes.ConstraintsPerTypeBatch);
constraintRemover = new ConstraintRemover(BufferPool, Bodies, Solver);
Sleeper = new IslandSleeper(Bodies, Solver, BroadPhase, constraintRemover, BufferPool);
Awakener = new IslandAwakener(Bodies, Statics, Solver, BroadPhase, Sleeper, bufferPool);
Statics.awakener = Awakener;
Solver.awakener = Awakener;
Bodies.Initialize(Solver, Awakener);
PoseIntegrator = new PoseIntegrator(Bodies, Shapes, BroadPhase);
SolverBatchCompressor = new BatchCompressor(Solver, Bodies);
BodyLayoutOptimizer = new BodyLayoutOptimizer(Bodies, BroadPhase, Solver, bufferPool);
ConstraintLayoutOptimizer = new ConstraintLayoutOptimizer(Bodies, Solver);
}
public BatchCompressor(Solver solver, Bodies bodies, float targetCandidateFraction = 0.01f, float maximumCompressionFraction = 0.0005f)
{
this.Solver = solver;
this.Bodies = bodies;
TargetCandidateFraction = targetCandidateFraction;
this.MaximumCompressionFraction = maximumCompressionFraction;
analysisWorkerDelegate = AnalysisWorker;
}
/// <summary>
/// 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.
/// </summary>
public void Clear()
{
Solver.Clear();
Bodies.Clear();
Statics.Clear();
Shapes.Clear();
BroadPhase.Clear();
}
public Deactivator(Bodies bodies, Solver solver, BufferPool pool)
{
this.bodies = bodies;
this.solver = solver;
this.pool = pool;
IdPool <Buffer <int> > .Create(pool.SpecializeFor <int>(), 16, out islandIdPool);
workDelegate = Work;
}
public ConstraintLayoutOptimizer(Bodies bodies, Solver solver, float optimizationFraction = 0.044f)
{
this.bodies = bodies;
this.solver = solver;
OptimizationFraction = optimizationFraction;
generateSortKeysDelegate = GenerateSortKeys;
regatherDelegate = Regather;
copyToCacheAndSortDelegate = CopyToCacheAndSort;
}
public unsafe Statics(BufferPool pool, Shapes shapes, Bodies bodies, BroadPhase broadPhase, int initialCapacity = 4096)
{
this.pool = pool;
InternalResize(Math.Max(1, initialCapacity));
this.shapes = shapes;
this.bodies = bodies;
this.broadPhase = broadPhase;
IdPool <Buffer <int> > .Create(pool.SpecializeFor <int>(), initialCapacity, out HandlePool);
}
public unsafe Statics(BufferPool pool, Shapes shapes, Bodies bodies, BroadPhase broadPhase, int initialCapacity = 4096)
{
this.pool = pool;
InternalResize(Math.Max(1, initialCapacity));
this.shapes = shapes;
this.bodies = bodies;
this.broadPhase = broadPhase;
HandlePool = new IdPool(initialCapacity, pool);
}
public IslandActivator(Bodies bodies, Statics statics, Solver solver, BroadPhase broadPhase, Deactivator deactivator, BufferPool pool)
{
this.bodies = bodies;
this.statics = statics;
this.solver = solver;
this.broadPhase = broadPhase;
this.deactivator = deactivator;
this.pool = pool;
this.phaseOneWorkerDelegate = PhaseOneWorker;
this.phaseTwoWorkerDelegate = PhaseTwoWorker;
}
// CONSTRAINTS
/// <summary>
/// Allocates a constraint slot and sets up a constraint with the specified description.
/// </summary>
/// <typeparam name="TDescription">Type of the constraint description to add.</typeparam>
/// <param name="bodyHandles">First body handle in a list of body handles used by the constraint.</param>
/// <param name="bodyCount">Number of bodies used by the constraint.</param>
/// <returns>Allocated constraint handle.</returns>
public int Add <TDescription>(ref int bodyHandles, int bodyCount, ref TDescription description)
where TDescription : IConstraintDescription <TDescription>
{
Solver.Add(ref bodyHandles, bodyCount, ref description, out int constraintHandle);
for (int i = 0; i < bodyCount; ++i)
{
var bodyHandle = Unsafe.Add(ref bodyHandles, i);
Bodies.ValidateExistingHandle(bodyHandle);
Bodies.AddConstraint(Bodies.HandleToLocation[bodyHandle].Index, constraintHandle, i);
}
return(constraintHandle);
}
public static void SwapBodyLocation(Bodies bodies, int a, int b)
{
Debug.Assert(a != b, "Swapping a body with itself isn't meaningful. Whaddeyer doin?");
//Enumerate the bodies' current set of constraints, changing the reference in each to the new location.
//Note that references to both bodies must be changed- both bodies moved!
//This function does not update the actual position of the list in the graph, so we can modify both without worrying about invalidating indices.
bodies.UpdateAttachedConstraintsForBodyMemoryMove(a, b);
bodies.UpdateAttachedConstraintsForBodyMemoryMove(b, a);
//Update the body locations.
bodies.ActiveSet.Swap(a, b, ref bodies.HandleToLocation);
}
public IslandAwakener(Bodies bodies, Statics statics, Solver solver, BroadPhase broadPhase, IslandSleeper sleeper, BufferPool pool)
{
this.bodies = bodies;
this.statics = statics;
this.solver = solver;
this.broadPhase = broadPhase;
this.sleeper = sleeper;
this.pool = pool;
this.phaseOneWorkerDelegate = PhaseOneWorker;
this.phaseTwoWorkerDelegate = PhaseTwoWorker;
}
public unsafe BoundingBoxBatcher(Bodies bodies, Shapes shapes, BroadPhase broadPhase, BufferPool pool, float dt)
{
this.bodies = bodies;
this.shapes = shapes;
this.broadPhase = broadPhase;
this.pool = pool;
this.dt = dt;
pool.SpecializeFor <QuickList <BoundingBoxInstance> >().Take(shapes.RegisteredTypeSpan, out batches);
//Clearing is required ensure that we know when a batch needs to be created and when a batch needs to be disposed.
batches.Clear(0, shapes.RegisteredTypeSpan);
minimumBatchIndex = shapes.RegisteredTypeSpan;
maximumBatchIndex = -1;
}
/// <summary>
/// Initializes the body property collection if the Bodies-less constructor was used.
/// </summary>
/// <param name="bodies">Bodies collection to track.</param>
public void Initialize(Bodies bodies)
{
if (this.bodies != null)
{
throw new InvalidOperationException("Initialize should only be used on a collection which was constructed without defining the Bodies collection.");
}
this.bodies = bodies;
if (pool == null)
{
pool = bodies.Pool;
}
pool.Take(bodies.HandleToLocation.Length, out data);
}
/// <summary>
/// Clears the simulation of every object and returns all pooled memory to the buffer pool. Leaves the simulation in an unusable state.
/// </summary>
public void Dispose()
{
Clear();
Activator.Dispose();
Deactivator.Dispose();
Solver.Dispose();
BroadPhase.Dispose();
NarrowPhase.Dispose();
Bodies.Dispose();
Statics.Dispose();
BodyLayoutOptimizer.Dispose(BufferPool);
Shapes.Dispose();
}
/// <summary>
/// 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.
/// </summary>
/// <remarks>
/// <para>
/// 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.
/// </para>
/// <para>
/// 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.
/// </para>
/// </remarks>
/// <param name="allocationTarget">Allocation sizes to guarantee sufficient size for.</param>
public void Resize(SimulationAllocationSizes allocationTarget)
{
Solver.ResizeSolverCapacities(allocationTarget.Bodies, allocationTarget.Constraints);
Solver.MinimumCapacityPerTypeBatch = allocationTarget.ConstraintsPerTypeBatch;
Solver.ResizeTypeBatchCapacities();
//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();
BodyLayoutOptimizer.ResizeForBodiesCapacity(BufferPool);
Statics.Resize(allocationTarget.Statics);
Shapes.ResizeBatches(allocationTarget.ShapesPerType);
BroadPhase.Resize(allocationTarget.Bodies, allocationTarget.Bodies + allocationTarget.Statics);
}
public static void SwapBodyLocation(Bodies bodies, Solver solver, int a, int b)
{
Debug.Assert(a != b, "Swapping a body with itself isn't meaningful. Whaddeyer doin?");
//Enumerate the bodies' current set of constraints, changing the reference in each to the new location.
//Note that references to both bodies must be changed- both bodies moved!
//This function does not update the actual position of the list in the graph, so we can modify both without worrying about invalidating indices.
solver.UpdateForBodyMemorySwap(a, b);
//Update the body locations.
bodies.ActiveSet.Swap(a, b, ref bodies.HandleToLocation);
//TODO: If the body layout optimizer occurs before or after all other stages, this swap isn't required. If we move it in between other stages though, we need to keep the inertia
//coherent with the other body properties.
//Helpers.Swap(ref bodies.Inertias[a], ref bodies.Inertias[b]);
}
public static void SwapBodyLocation(Bodies bodies, Solver solver, int a, int b)
{
Debug.Assert(a != b, "Swapping a body with itself isn't meaningful. Whaddeyer doin?");
//Enumerate the bodies' current set of constraints, changing the reference in each to the new location.
//Note that references to both bodies must be changed- both bodies moved!
//This function does not update the actual position of the list in the graph, so we can modify both without worrying about invalidating indices.
solver.UpdateForBodyMemorySwap(a, b);
//Update the body locations.
bodies.ActiveSet.Swap(a, b, ref bodies.HandleToLocation);
//Note that the active set does not contain the world inertia. While this isn't actually a problem for correctness so long as the body layout optimizer
//takes places before the pose integrator or after all stages, it does create a pretty confusing dependency which is easy to forget.
//Given how cheap it is, we're just going to swap the world inertia too.
Helpers.Swap(ref bodies.Inertias[a], ref bodies.Inertias[b]);
}
/// <summary>
/// Constructs a new body reference.
/// </summary>
/// <param name="handle">Handle of the body to refer to.</param>
/// <param name="bodies">Collection containing the body.</param>
public BodyReference(int handle, Bodies bodies)
{
Handle = handle;
Bodies = bodies;
}
/// <summary>
/// Constructs a new collection to store handle-aligned body properties.
/// </summary>
/// <param name="bodies">Bodies collection to track.</param>
/// <param name="pool">Pool from which to pull internal resources. If null, uses the Bodies pool.</param>
public BodyProperty(Bodies bodies, BufferPool pool = null)
{
this.bodies = bodies;
this.pool = pool == null ? bodies.Pool : pool;
pool.Take(bodies.HandleToLocation.Length, out data);
}
public int GetBodyReference(Bodies bodies, int bodyHandle)
{
ref var bodyLocation = ref bodies.HandleToLocation[bodyHandle];
unsafe void Allocate <TBodyReferenceGetter>(int constraintHandle, ref int constraintBodyHandles, int bodyCount, Bodies bodies,
int typeId, BufferPool pool, TBodyReferenceGetter bodyReferenceGetter, int minimumBodyCapacity, int minimumReferenceCapacity)
where TBodyReferenceGetter : struct, IBodyReferenceGetter
{
EnsureCapacity(Math.Max(bodyConstraintReferences.Count + bodyCount, minimumBodyCapacity), pool);
for (int i = 0; i < bodyCount; ++i)
{
var bodyReference = bodyReferenceGetter.GetBodyReference(bodies, Unsafe.Add(ref constraintBodyHandles, i));
var bodyAlreadyListed = bodyConstraintReferences.GetTableIndices(ref bodyReference, out var tableIndex, out var elementIndex);
//If an entry for this body does not yet exist, we'll create one.
if (!bodyAlreadyListed)
{
elementIndex = bodyConstraintReferences.Count;
}
ref var constraintReferences = ref bodyConstraintReferences.Values[elementIndex];
if (!bodyAlreadyListed)
{
//The body is not already contained. Create a list for it.
constraintReferences = new QuickSet <FallbackReference, FallbackReferenceComparer>(minimumReferenceCapacity, pool);
bodyConstraintReferences.Keys[elementIndex] = bodyReference;
bodyConstraintReferences.Table[tableIndex] = elementIndex + 1;
++bodyConstraintReferences.Count;
}
var fallbackReference = new FallbackReference {
ConstraintHandle = constraintHandle, IndexInConstraint = i
};
constraintReferences.AddRef(ref fallbackReference, pool);
}
public ActiveConstraintBodyHandleCollector(Bodies bodies, int *handles)
{
Bodies = bodies;
Handles = handles;
Index = 0;
}