protected Simulation(BufferPool bufferPool, SimulationAllocationSizes initialAllocationSizes, int solverIterationCount, int solverFallbackBatchThreshold, ITimestepper timestepper)
{
BufferPool = bufferPool;
PhysicsShapes3D = new PhysicsShapes3D(bufferPool, initialAllocationSizes.ShapesPerType);
BroadPhase = new BroadPhase(bufferPool, initialAllocationSizes.Bodies, initialAllocationSizes.Bodies + initialAllocationSizes.Statics);
Bodies3D = new Bodies3D(bufferPool, PhysicsShapes3D, BroadPhase,
initialAllocationSizes.Bodies,
initialAllocationSizes.Islands,
initialAllocationSizes.ConstraintCountPerBodyEstimate);
Statics = new Statics(bufferPool, PhysicsShapes3D, Bodies3D, BroadPhase, initialAllocationSizes.Statics);
Solver = new Solver(Bodies3D, BufferPool, solverIterationCount, solverFallbackBatchThreshold,
initialCapacity: initialAllocationSizes.Constraints,
initialIslandCapacity: initialAllocationSizes.Islands,
minimumCapacityPerTypeBatch: initialAllocationSizes.ConstraintsPerTypeBatch);
constraintRemover = new ConstraintRemover(BufferPool, Bodies3D, Solver);
Sleeper = new IslandSleeper(Bodies3D, Solver, BroadPhase, constraintRemover, BufferPool);
Awakener = new IslandAwakener(Bodies3D, Statics, Solver, BroadPhase, Sleeper, bufferPool);
Statics.awakener = Awakener;
Solver.awakener = Awakener;
Bodies3D.Initialize(Solver, Awakener, Sleeper);
SolverBatchCompressor = new BatchCompressor(Solver, Bodies3D);
BodyLayoutOptimizer = new BodyLayoutOptimizer(Bodies3D, BroadPhase, Solver, bufferPool);
ConstraintLayoutOptimizer = new ConstraintLayoutOptimizer(Bodies3D, Solver);
Timestepper = timestepper;
}
public BodyLayoutOptimizer(Bodies3D bodies3D, BroadPhase broadPhase, Solver solver, BufferPool pool, float optimizationFraction = 0.005f)
{
this.bodies3D = bodies3D;
this.broadPhase = broadPhase;
this.solver = solver;
OptimizationFraction = optimizationFraction;
}
public BatchCompressor(Solver solver, Bodies3D bodies3D, float targetCandidateFraction = 0.01f, float maximumCompressionFraction = 0.0005f)
{
this.Solver = solver;
this.Bodies3D = bodies3D;
TargetCandidateFraction = targetCandidateFraction;
this.MaximumCompressionFraction = maximumCompressionFraction;
analysisWorkerDelegate = AnalysisWorker;
}
public ConstraintLayoutOptimizer(Bodies3D bodies3D, Solver solver, float optimizationFraction = 0.044f)
{
this.bodies3D = bodies3D;
this.solver = solver;
OptimizationFraction = optimizationFraction;
generateSortKeysDelegate = GenerateSortKeys;
regatherDelegate = Regather;
copyToCacheAndSortDelegate = CopyToCacheAndSort;
}
public unsafe Statics(BufferPool pool, PhysicsShapes3D shapes, Bodies3D bodies3D, BroadPhase broadPhase, int initialCapacity = 4096)
{
this.pool = pool;
InternalResize(Math.Max(1, initialCapacity));
this.shapes = shapes;
this.bodies3D = bodies3D;
this.broadPhase = broadPhase;
HandlePool = new IdPool(initialCapacity, pool);
}
public IslandAwakener(Bodies3D bodies3D, Statics statics, Solver solver, BroadPhase broadPhase, IslandSleeper sleeper, BufferPool pool)
{
this.bodies3D = bodies3D;
this.statics = statics;
this.solver = solver;
this.broadPhase = broadPhase;
this.sleeper = sleeper;
this.pool = pool;
this.phaseOneWorkerDelegate = PhaseOneWorker;
this.phaseTwoWorkerDelegate = PhaseTwoWorker;
}
/// <summary>
/// Initializes the body property collection if the Bodies3D-less constructor was used.
/// </summary>
/// <param name="bodies3D">Bodies3D collection to track.</param>
public void Initialize(Bodies3D bodies3D)
{
if (this.bodies3D != null)
{
throw new InvalidOperationException("Initialize should only be used on a collection which was constructed without defining the Bodies3D collection.");
}
this.bodies3D = bodies3D;
if (pool == null)
{
pool = bodies3D.Pool;
}
pool.TakeAtLeast(bodies3D.HandleToLocation.Length, out data);
}
public unsafe BoundingBoxBatcher(Bodies3D bodies3D, PhysicsShapes3D shapes, BroadPhase broadPhase, BufferPool pool, float dt)
{
this.bodies3D = bodies3D;
this.shapes = shapes;
this.broadPhase = broadPhase;
this.pool = pool;
this.dt = dt;
pool.TakeAtLeast(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;
}
public static void SwapBodyLocation(Bodies3D bodies3D, Solver solver, int a, int b)
{
Debug.Assert(a != b, "Swapping a body with itself isn't meaningful. Whaddeyer doin?");
//Enumerate the bodies3D' current set of constraints, changing the reference in each to the new location.
//Note that references to both bodies3D must be changed- both bodies3D 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.
bodies3D.ActiveSet.Swap(a, b, ref bodies3D.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 bodies3D.Inertias[a], ref bodies3D.Inertias[b]);
}
public abstract void Prestep(ref TypeBatch typeBatch, Bodies3D bodies3D, float dt, float inverseDt, int startBundle, int exclusiveEndBundle);
unsafe void Allocate <TBodyReferenceGetter>(int constraintHandle, ref int constraintBodyHandles, int bodyCount, Bodies3D bodies3D,
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(bodies3D, 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(Bodies3D bodies3D, int *handles)
{
this.bodies3D = bodies3D;
Handles = handles;
Index = 0;
}
/// <summary>
/// Constructs a new body reference.
/// </summary>
/// <param name="handle">Handle of the body to refer to.</param>
/// <param name="bodies3D">Collection containing the body.</param>
public BodyReference(int handle, Bodies3D bodies3D)
{
Handle = handle;
this.bodies3D = bodies3D;
}
public int GetBodyReference(Bodies3D bodies3D, int bodyHandle)
{
ref var bodyLocation = ref bodies3D.HandleToLocation[bodyHandle];
//The following covers the common loop logic for all one body constraints. Each iteration invokes the warm start function type.
//This abstraction should, in theory, have zero overhead if the implementation of the interface is in a struct with aggressive inlining.
//By providing the overrides at this level, the concrete implementation (assuming it inherits from one of the prestep-providing variants)
//only has to specify *type* arguments associated with the interface-implementing struct-delegates. It's going to look very strange, but it's low overhead
//and minimizes per-type duplication.
public unsafe override void Prestep(ref TypeBatch typeBatch, Bodies3D bodies3D, float dt, float inverseDt, int startBundle, int exclusiveEndBundle)
{
ref var prestepBase = ref Unsafe.AsRef <TPrestepData>(typeBatch.PrestepData.Memory);
/// <summary> /// Moves a constraint from one ConstraintBatch's TypeBatch to another ConstraintBatch's TypeBatch of the same type. /// </summary> /// <param name="sourceBatchIndex">Index of the batch that owns the type batch that is the source of the constraint transfer.</param> /// <param name="indexInTypeBatch">Index of the constraint to move in the current type batch.</param> /// <param name="solver">Solver that owns the batches.</param> /// <param name="bodies3D">Bodies3D set that owns all the constraint's bodies3D.</param> /// <param name="targetBatchIndex">Index of the ConstraintBatch in the solver to copy the constraint into.</param> public unsafe abstract void TransferConstraint(ref TypeBatch typeBatch, int sourceBatchIndex, int indexInTypeBatch, Solver solver, Bodies3D bodies3D, int targetBatchIndex);
public virtual void IncrementallyUpdateContactData(ref TypeBatch typeBatch, Bodies3D bodies3D, float dt, float inverseDt, int startBundle, int end)
{
Debug.Fail("A contact data update was scheduled for a type batch that does not have a contact data update implementation.");
}
internal unsafe abstract void GatherActiveConstraints(Bodies3D bodies3D, Solver solver, ref QuickList <int> sourceHandles, int startIndex, int endIndex, ref TypeBatch targetTypeBatch);
internal unsafe abstract void CopySleepingToActive(
int sourceSet, int sourceBatchIndex, int sourceTypeBatchIndex, int targetBatchIndex, int targetTypeBatchIndex,
int sourceStart, int targetStart, int count, Bodies3D bodies3D, Solver solver);
public abstract void JacobiPrestep(ref TypeBatch typeBatch, Bodies3D bodies3D, ref FallbackBatch jacobiBatch, float dt, float inverseDt, int startBundle, int exclusiveEndBundle);
/// <summary>
/// Constructs a new collection to store handle-aligned body properties.
/// </summary>
/// <param name="bodies3D">Bodies3D collection to track.</param>
/// <param name="pool">Pool from which to pull internal resources. If null, uses the Bodies3D pool.</param>
public BodyProperty(Bodies3D bodies3D, BufferPool pool = null)
{
this.bodies3D = bodies3D;
this.pool = pool == null ? bodies3D.Pool : pool;
this.pool.TakeAtLeast(bodies3D.HandleToLocation.Length, out data);
}