unsafe void Allocate <TBodyReferenceGetter>(ConstraintHandle constraintHandle, Span <BodyHandle> constraintBodyHandles, Bodies bodies,
int typeId, BufferPool pool, TBodyReferenceGetter bodyReferenceGetter, int minimumBodyCapacity, int minimumReferenceCapacity)
where TBodyReferenceGetter : struct, IBodyReferenceGetter
{
EnsureCapacity(Math.Max(bodyConstraintReferences.Count + constraintBodyHandles.Length, minimumBodyCapacity), pool);
for (int i = 0; i < constraintBodyHandles.Length; ++i)
{
var bodyReference = bodyReferenceGetter.GetBodyReference(bodies, 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);
}
/// <summary>
/// Wakes up any sleeping bodies associated with a constraint. All bodies that can be found by traversing the constraint graph from the constraint referenced bodies will also be awakened.
/// If all bodies associated with the constraint are already awake, this does nothing.
/// </summary>
/// <param name="constraintHandle">Handle of the constraint to awaken.</param>
public void AwakenConstraint(ConstraintHandle constraintHandle)
{
AwakenSet(solver.HandleToConstraint[constraintHandle.Value].SetIndex);
}
