static Context GetFreshContext(int constraintBundleCount, int bodyCount, Comparison <ConstraintBodies> sortComparison = null)
{
Context context;
context.ConstraintBundleCount = constraintBundleCount;
var pool = new BufferPool();
pool.SpecializeFor <BodyVelocity>().Take(bodyCount, out context.BodyVelocities);
var connections = new ConstraintBodies[constraintBundleCount * Vector <int> .Count];
var random = new Random(5);
for (int i = 0; i < connections.Length; ++i)
{
connections[i] = new ConstraintBodies {
A = random.Next(bodyCount), B = random.Next(bodyCount)
};
}
if (sortComparison != null)
{
//Sorting the connections should increase the probability that at least one of the two bodies associated with a constraint will be in the cache already from earlier prefetches.
Array.Sort(connections, sortComparison);
}
context.BodyReferences = new TwoBodyReferences[constraintBundleCount];
for (int iterationIndex = 0; iterationIndex < constraintBundleCount; ++iterationIndex)
{
var baseSourceIndex = iterationIndex << BundleIndexing.VectorShift;
for (int i = 0; i < Vector <int> .Count; ++i)
{
var c = connections[baseSourceIndex + i];
GatherScatter.Get(ref context.BodyReferences[iterationIndex].IndexA, i) = c.A;
GatherScatter.Get(ref context.BodyReferences[iterationIndex].IndexB, i) = c.B;
}
}
return(context);
}
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));
}
unsafe static float[] TestWrappedVectorAccess(int iterationCount)
{
var vector = new Vector <float>();
float[] accumulators = new float[Vector <float> .Count];
int[] indices = new int[] { 0, 1, 2, 3 };
for (int iterationIndex = 0; iterationIndex < iterationCount; ++iterationIndex)
{
accumulators[0] += GatherScatter.Get(ref vector, indices[0]);
accumulators[1] += GatherScatter.Get(ref vector, indices[1]);
accumulators[2] += GatherScatter.Get(ref vector, indices[2]);
accumulators[3] += GatherScatter.Get(ref vector, indices[3]);
}
return(accumulators);
}
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));
}