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); }