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