private int ValidateAndCountShapefulBodies(ref BodySet bodySet, ref Tree tree, ref Buffer <CollidableReference> leaves)
{
int shapefulBodyCount = 0;
for (int i = 0; i < bodySet.Count; ++i)
{
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.Handle == bodySet.IndexToHandle[i]);
Debug.Assert(leaf.Mobility == CollidableMobility.Dynamic || leaf.Mobility == CollidableMobility.Kinematic);
++shapefulBodyCount;
}
/// <summary>
/// Constructs a new bodies collection. Initialize must be called for the instance to be ready for use.
/// </summary>
/// <param name="pool">Pool for the collection to pull persistent allocations from.</param>
/// <param name="shapes">Shapes referenced by the collection's bodies.</param>
/// <param name="broadPhase">Broad phase containing the body collidables.</param>
/// <param name="initialBodyCapacity">Initial number of bodies to allocate space for in the active set.</param>
/// <param name="initialIslandCapacity">Initial number of islands to allocate space for in the Sets buffer.</param>
/// <param name="initialConstraintCapacityPerBody">Expected number of constraint references per body to allocate space for.</param>
public unsafe Bodies(BufferPool pool, Shapes shapes, BroadPhase broadPhase,
int initialBodyCapacity, int initialIslandCapacity, int initialConstraintCapacityPerBody)
{
this.pool = pool;
//Note that the id pool only grows upon removal, so this is just a heuristic initialization.
//You could get by with something a lot less aggressive, but it does tend to avoid resizes in the case of extreme churn.
IdPool <Buffer <int> > .Create(pool.SpecializeFor <int>(), initialBodyCapacity, out HandlePool);
ResizeHandles(initialBodyCapacity);
ResizeSetsCapacity(initialIslandCapacity + 1, 0);
ActiveSet = new BodySet(initialBodyCapacity, pool);
this.shapes = shapes;
this.broadPhase = broadPhase;
MinimumConstraintCapacityPerBody = initialConstraintCapacityPerBody;
}