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;