public int GetSortKey(int constraintIndex, ref Buffer <Vector <int> > bodyReferences)
{
BundleIndexing.GetBundleIndices(constraintIndex, out var bundleIndex, out var innerIndex);
//We sort based on the body references within the constraint.
//Note that it is impossible for there to be two references to the same body within a constraint batch,
//so there's no need to worry about the case where the comparison is equal.
return(GatherScatter.Get(ref bodyReferences[bundleIndex], innerIndex));
}
public unsafe void EnumerateAccumulatedImpulses <TEnumerator>(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator) where TEnumerator : IForEach <float>
{
BundleIndexing.GetBundleIndices(indexInTypeBatch, out var bundleIndex, out var innerIndex);
var bundleSizeInFloats = ConstrainedDegreesOfFreedom * Vector <float> .Count;
ValidateAccumulatedImpulsesSizeInBytes(bundleSizeInFloats * 4);
var impulseAddress = (float *)typeBatch.AccumulatedImpulses.Memory + (bundleIndex * bundleSizeInFloats + innerIndex);
enumerator.LoopBody(*impulseAddress);
for (int i = 1; i < ConstrainedDegreesOfFreedom; ++i)
{
impulseAddress += Vector <float> .Count;
enumerator.LoopBody(*impulseAddress);
}
}
public sealed unsafe override void EnumerateConnectedBodyIndices <TEnumerator>(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator)
{
BundleIndexing.GetBundleIndices(indexInTypeBatch, out var constraintBundleIndex, out var constraintInnerIndex);
ref var indices = ref GatherScatter.GetOffsetInstance(ref Buffer <FourBodyReferences> .Get(typeBatch.BodyReferences.Memory, constraintBundleIndex), constraintInnerIndex);
public void GetPoint(int pointIndex, out Vector3 point)
{
BundleIndexing.GetBundleIndices(pointIndex, out var bundleIndex, out var innerIndex);
Vector3Wide.ReadSlot(ref Points[bundleIndex], innerIndex, out point);
}
public sealed override void EnumerateConnectedBodyIndices <TEnumerator>(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator)
{
BundleIndexing.GetBundleIndices(indexInTypeBatch, out var constraintBundleIndex, out var constraintInnerIndex);
ref var indexA = ref GatherScatter.Get(ref Buffer <TwoBodyReferences> .Get(ref typeBatch.BodyReferences, constraintBundleIndex).IndexA, constraintInnerIndex);
public unsafe void GatherOldImpulses(ref ConstraintReference constraintReference, float *oldImpulses)
{
BundleIndexing.GetBundleIndices(constraintReference.IndexInTypeBatch, out var bundleIndex, out var inner);
ref var buffer = ref constraintReference.TypeBatch.AccumulatedImpulses;
public sealed override void EnumerateConnectedBodyIndices <TEnumerator>(ref TypeBatch typeBatch, int indexInTypeBatch, ref TEnumerator enumerator)
{
BundleIndexing.GetBundleIndices(indexInTypeBatch, out var constraintBundleIndex, out var constraintInnerIndex);
enumerator.LoopBody(GatherScatter.Get(ref Buffer <Vector <int> > .Get(ref typeBatch.BodyReferences, constraintBundleIndex), constraintInnerIndex));
}
public unsafe void GatherOldImpulses(ref ConstraintReference constraintReference, float *oldImpulses)
{
//Note that we do not modify the friction accumulated impulses. This is just for simplicity- the impact of accumulated impulses on friction *should* be relatively
//hard to notice compared to penetration impulses. TODO: We should, however, test this assumption.
BundleIndexing.GetBundleIndices(constraintReference.IndexInTypeBatch, out var bundleIndex, out var inner);
ref var buffer = ref constraintReference.TypeBatch.AccumulatedImpulses;
