public unsafe void ExtractLines(ref CenterDistancePrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
var targetDistance = GatherScatter.GetFirst(ref prestepBundle.TargetDistance);
var color = new Vector3(0.2f, 0.2f, 1f) * tint;
var packedColor = Helpers.PackColor(color);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
//Draw a line from A to B. If the true distance is longer than the target distance, draw a red line to complete the gap.
//If the true distance is shorter than the target distance, draw an overshooting red line.
var offset = poseB.Position - poseA.Position;
var length = offset.Length();
var direction = length < 1e-9f ? new Vector3(1, 0, 0) : offset / length;
var errorColor = new Vector3(1, 0, 0) * tint;
var packedErrorColor = Helpers.PackColor(errorColor);
var packedDistanceColor = Helpers.PackColor(color * 0.5f);
var targetEnd = poseA.Position + direction * targetDistance;
if (length < targetDistance)
{
lines.AllocateUnsafely() = new LineInstance(poseA.Position, poseB.Position, packedDistanceColor, 0);
lines.AllocateUnsafely() = new LineInstance(poseB.Position, targetEnd, packedErrorColor, 0);
}
else
{
lines.AllocateUnsafely() = new LineInstance(poseA.Position, targetEnd, packedDistanceColor, 0);
lines.AllocateUnsafely() = new LineInstance(targetEnd, poseB.Position, packedErrorColor, 0);
}
}
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));
}
public unsafe void FindLocalOverlaps(ref Buffer <BoundsTestedPair> pairs, int pairCount, BufferPool pool, Shapes shapes, float dt, out ConvexCompoundTaskOverlaps overlaps)
{
overlaps = new ConvexCompoundTaskOverlaps(pool, pairCount);
var pairsToTest = stackalloc PairsToTestForOverlap[pairCount];
Vector3Wide offsetB = default;
QuaternionWide orientationA = default;
QuaternionWide orientationB = default;
Vector3Wide relativeLinearVelocityA = default;
Vector3Wide angularVelocityA = default;
Vector3Wide angularVelocityB = default;
Vector <float> maximumAllowedExpansion = default;
TConvexWide convexWide = default;
for (int i = 0; i < pairCount; i += Vector <float> .Count)
{
var count = pairCount - i;
if (count > Vector <float> .Count)
{
count = Vector <float> .Count;
}
//Compute the local bounding boxes using wide operations for the expansion work.
//Doing quite a bit of gather work (and still quite a bit of scalar work). Very possible that a scalar path could win. TODO: test that.
for (int j = 0; j < count; ++j)
{
var pairIndex = i + j;
ref var pair = ref pairs[pairIndex];
pairsToTest[pairIndex].Container = pair.B;
Vector3Wide.WriteFirst(pair.OffsetB, ref GatherScatter.GetOffsetInstance(ref offsetB, j));
QuaternionWide.WriteFirst(pair.OrientationA, ref GatherScatter.GetOffsetInstance(ref orientationA, j));
QuaternionWide.WriteFirst(pair.OrientationB, ref GatherScatter.GetOffsetInstance(ref orientationB, j));
Vector3Wide.WriteFirst(pair.RelativeLinearVelocityA, ref GatherScatter.GetOffsetInstance(ref relativeLinearVelocityA, j));
Vector3Wide.WriteFirst(pair.AngularVelocityA, ref GatherScatter.GetOffsetInstance(ref angularVelocityA, j));
Vector3Wide.WriteFirst(pair.AngularVelocityB, ref GatherScatter.GetOffsetInstance(ref angularVelocityB, j));
Unsafe.Add(ref Unsafe.As <Vector <float>, float>(ref maximumAllowedExpansion), j) = pair.MaximumExpansion;
GatherScatter.GetOffsetInstance(ref convexWide, j).WriteFirst(ref Unsafe.AsRef <TConvex>(pair.A));
}
QuaternionWide.Conjugate(orientationB, out var inverseOrientationB);
QuaternionWide.TransformWithoutOverlap(offsetB, inverseOrientationB, out var localOffsetB);
QuaternionWide.ConcatenateWithoutOverlap(orientationA, inverseOrientationB, out var localOrientationA);
QuaternionWide.TransformWithoutOverlap(relativeLinearVelocityA, inverseOrientationB, out var localRelativeLinearVelocityA);
convexWide.GetBounds(ref localOrientationA, out var maximumRadius, out var maximumAngularExpansion, out var min, out var max);
Vector3Wide.Negate(localOffsetB, out var localPositionA);
BoundingBoxHelpers.ExpandLocalBoundingBoxes(ref min, ref max, Vector <float> .Zero, localPositionA, localRelativeLinearVelocityA, angularVelocityA, angularVelocityB, dt,
maximumRadius, maximumAngularExpansion, maximumAllowedExpansion);
for (int j = 0; j < count; ++j)
{
ref var pairToTest = ref pairsToTest[i + j];
Vector3Wide.ReadSlot(ref min, j, out pairToTest.Min);
Vector3Wide.ReadSlot(ref max, j, out pairToTest.Max);
}
static void TestScatter(Context context)
{
var a = new BodyVelocities();
var b = new BodyVelocities();
for (int i = 0; i < context.ConstraintBundleCount; ++i)
{
GatherScatter.ScatterVelocities(ref context.BodyVelocities, ref context.BodyReferences[i], Vector <float> .Count, ref a, ref b);
}
}
public unsafe void ExtractLines(ref LinearAxisServoPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA);
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
Vector3Wide.ReadFirst(prestepBundle.LocalPlaneNormal, out var localPlaneNormal);
var targetOffset = GatherScatter.GetFirst(ref prestepBundle.TargetOffset);
Matrix3x3.CreateFromQuaternion(poseA.Orientation, out var orientationA);
Matrix3x3.Transform(localOffsetA, orientationA, out var worldOffsetA);
Matrix3x3.Transform(localPlaneNormal, orientationA, out var worldPlaneNormal);
Quaternion.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var anchorA = poseA.Position + worldOffsetA;
var anchorB = poseB.Position + worldOffsetB;
var planeOffset = Vector3.Dot(anchorB - anchorA, worldPlaneNormal);
var closestPointOnPlane = anchorB - planeOffset * worldPlaneNormal;
var packedColor = Helpers.PackColor(new Vector3(0.2f, 0.2f, 1f) * tint);
var packedBasisColor = Helpers.PackColor(new Vector3(0.2f, 0.6f, 1f) * tint);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, anchorA, packedColor, 0);
ContactLines.BuildOrthnormalBasis(localPlaneNormal, out var localTX, out var localTY);
Matrix3x3.Transform(localTX, orientationA, out var tX);
Matrix3x3.Transform(localTY, orientationA, out var tY);
lines.AllocateUnsafely() = new LineInstance(anchorA - tX, anchorA + tX, packedBasisColor, 0);
lines.AllocateUnsafely() = new LineInstance(anchorA - tY, anchorA + tY, packedBasisColor, 0);
lines.AllocateUnsafely() = new LineInstance(anchorA, closestPointOnPlane, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(anchorB, poseB.Position, packedColor, 0);
if (targetOffset < 0)
{
targetOffset = -targetOffset;
planeOffset = -planeOffset;
worldPlaneNormal = -worldPlaneNormal;
}
var targetPoint = closestPointOnPlane + worldPlaneNormal * targetOffset;
var packedErrorColor = Helpers.PackColor(new Vector3(1, 0, 0) * tint);
if (planeOffset > targetOffset)
{
lines.AllocateUnsafely() = new LineInstance(closestPointOnPlane, targetPoint, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(targetPoint, anchorB, packedErrorColor, 0);
}
else
{
lines.AllocateUnsafely() = new LineInstance(closestPointOnPlane, anchorB, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(anchorB, targetPoint, packedErrorColor, 0);
}
}
public unsafe void ExtractLines(ref DistanceLimitPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA);
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
var minimumDistance = GatherScatter.GetFirst(ref prestepBundle.MinimumDistance);
var maximumDistance = GatherScatter.GetFirst(ref prestepBundle.MaximumDistance);
QuaternionEx.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA);
QuaternionEx.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var endA = poseA.Position + worldOffsetA;
var endB = poseB.Position + worldOffsetB;
var color = new Vector3(0.2f, 0.2f, 1f) * tint;
var packedColor = Helpers.PackColor(color);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, endA, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(poseB.Position, endB, packedColor, 0);
var offset = endB - endA;
var length = offset.Length();
var direction = length < 1e-9f ? new Vector3(1, 0, 0) : offset / length;
var errorColor = new Vector3(1, 0, 0) * tint;
var packedErrorColor = Helpers.PackColor(errorColor);
var packedFarColor = Helpers.PackColor(color * 0.5f);
var packedNearColor = Helpers.PackColor(color * 0.25f);
var minimumPoint = endA + direction * minimumDistance;
if (length >= minimumDistance && length <= maximumDistance)
{
//Create a darker bar to signify the minimum limit.
lines.AllocateUnsafely() = new LineInstance(endA, minimumPoint, packedNearColor, 0);
lines.AllocateUnsafely() = new LineInstance(minimumPoint, endB, packedFarColor, 0);
lines.AllocateUnsafely() = new LineInstance(new Vector3(float.MinValue), new Vector3(float.MinValue), 0, 0);
}
else if (length < minimumDistance)
{
//Too close; draw an error line extending beyond the connecting line.
lines.AllocateUnsafely() = new LineInstance(endA, endB, packedNearColor, 0);
lines.AllocateUnsafely() = new LineInstance(endB, endA + direction * minimumDistance, packedErrorColor, 0);
lines.AllocateUnsafely() = new LineInstance(new Vector3(float.MinValue), new Vector3(float.MinValue), 0, 0);
}
else
{
//Too far; draw an error line that extends from the desired endpoint to the current endpoint.
var targetEnd = endA + direction * maximumDistance;
lines.AllocateUnsafely() = new LineInstance(endA, minimumPoint, packedNearColor, 0);
lines.AllocateUnsafely() = new LineInstance(minimumPoint, targetEnd, packedFarColor, 0);
lines.AllocateUnsafely() = new LineInstance(targetEnd, endB, packedErrorColor, 0);
}
}
void Append(ref Vector <int> mask, ref Simplex simplex, ref Vector3Wide vA, ref Vector3Wide v)
{
for (int i = 0; i < Vector <float> .Count; ++i)
{
if (mask[i] == 0)
{
ref var count = ref Unsafe.Add(ref GatherScatter.GetFirst(ref simplex.Count), i);
ref var vTargetSlot = ref GatherScatter.GetOffsetInstance(ref Unsafe.Add(ref simplex.A, count), i);
GatherScatter.GetFirst(ref vTargetSlot.X) = v.X[i];
GatherScatter.GetFirst(ref vTargetSlot.Y) = v.Y[i];
GatherScatter.GetFirst(ref vTargetSlot.Z) = v.Z[i];
ref var vATargetSlot = ref GatherScatter.GetOffsetInstance(ref Unsafe.Add(ref simplex.AOnA, count), i);
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 unsafe void ExtractLines(ref DistanceServoPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA);
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
var targetDistance = GatherScatter.GetFirst(ref prestepBundle.TargetDistance);
Quaternion.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA);
Quaternion.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var endA = poseA.Position + worldOffsetA;
var endB = poseB.Position + worldOffsetB;
var color = new Vector3(0.2f, 0.2f, 1f) * tint;
var packedColor = Helpers.PackColor(color);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, endA, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(poseB.Position, endB, packedColor, 0);
//Draw a line from A to B. If the true distance is longer than the target distance, draw a red line to complete the gap.
//If the true distance is shorter than the target distance, draw an overshooting red line.
var offset = endB - endA;
var length = offset.Length();
var direction = length < 1e-9f ? new Vector3(1, 0, 0) : offset / length;
var errorColor = new Vector3(1, 0, 0) * tint;
var packedErrorColor = Helpers.PackColor(errorColor);
var packedDistanceColor = Helpers.PackColor(color * 0.5f);
var targetEnd = endA + direction * targetDistance;
if (length < targetDistance)
{
lines.AllocateUnsafely() = new LineInstance(endA, endB, packedDistanceColor, 0);
lines.AllocateUnsafely() = new LineInstance(endB, targetEnd, packedErrorColor, 0);
}
else
{
lines.AllocateUnsafely() = new LineInstance(endA, targetEnd, packedDistanceColor, 0);
lines.AllocateUnsafely() = new LineInstance(targetEnd, endB, packedErrorColor, 0);
}
}
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));
}